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on  an  IBM  3033  digital  computer  and  can  accommodate  most  physical 
robotic  configurations.  Additionally,  an  attempt  has  been  made  to 
simulate  manipulator  motion  using  the  verified  model  and  previously 
published  integration  methods.  Comparison  of  simulation  results 
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I.   INTRODUCTION 

A.  MOTIVATION  FOR  SIMULATION 

The  U.S.  Navy  is  developing  robots  to  help  meet  the  Navy's 
automation  needs.  According  to  VADM  Fowler  [Ref.  1],  '"The 
construction  of  the  600-ship  (US)  Navy  requires  ever-improving 
productivity  through  the  adoption  of  the  most  cost-efficient 
manufacturing  technology.*'  A  robot,  as  defined  by  the  Robot 
Institute  of  America  (RIA)  [Ref.  2],  is:  ''a  reprogrammable 
multifunctional  manipulator  designed  to  move  material,  parts,  tools, 
or  specialized  devices,  through  variable  programmed  motions  for  the 
performance  of  a  variety  of  tasks.*'  Robot  manipulator  simulation 
allows  the  determination  of  arm  dynamic  responses  to  various 
permutations  of  input  data  by  mathematically  modelling  the  proposed 
configurations. 

B.  MODELLING  AND  SIMULATION  IN  DESIGN 

Simulation  software  can  model  diverse  components  of  a  robot,  even 
within  the  context  of  an  entire  robotic  manufacturing  cell  which  is 
comprised  of  the  robot  manipulator,  end  effector,  and  workspace 
elements.  Engineers  can  use  this  software  to  predict  dynamic 
performance  long  before  expensive  mechanisms  have  been  constructed. 
Physical  parameters  and  design  configurations  can  be  selected  and 
optimized,   and  complicated   control   schemes   can  be  modelled   and 
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evaluated.  Additionally,  engineers  can  focus  their  research 
attention  on  the  inherent  nonlinear  kinematic  and  dynamic  physical 
relationships  of  a  robot  manipulator  arm  to  its  workcell. 

1.  Performance  Prediction 

The  dynamic  response  predicted  by  a  manipulator  arm  simulator 
can  be  of  inestimable  value  to  robotic  engineers,  designers,  and 
users  in  many  areas  involving  the  effects  of  physical  design  values 
on  performance.  For  instance,  accurate  modelling  and  simulation  can 
be  used  in: 

1.  manipulator  arm  design 

2.  robotics  educational  training 

3.  manipulator  arm  selection 

4.  performance  evaluation 

5.  trajectory  planning 

6.  trajectory  evaluation 

7.  workspace  layout 

8.  control  programming 

9.  singularity  point  determination 

10.  cycle  time  determination 

11.  dynamic  property  calculation 

12.  actuator/link  loads  and  size  determination 

13.  error  effect  evaluation  (i.e.  backlash) 

14.  actuator  overload  predictions 

15.  parameter  optimization 

2.  Design  Parameter  Selection 

Manipulator  dynamic  analysis  must  aid  in  the  mechanical  and 
structural  design  of  prototype  arms.  Modelling  and  simulation  can 
hasten  robot  arm  development  by  permitting  an  engineer  to  examine 
kinematic  and  dynamic  behavior  and  better  understand  the  significance 
of  various  parameters  before  constructing  a  manipulator  arm.  The 
forces  and  torques  predicted  for  a  mechanical  arm  during  movement  can 
yield  data  useful  in  designing  actuators,  joints,  and  structures  by 
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making  the  role  of  primary  design  parameters  explicit.  Discrete 
dynamic  coefficients  can  be  numerically  evaluated  to  determine  their 
relative  magnitude  and  importance.  Additionally,  a  valid  model  can 
be  used  to  identify  negligible  dynamics  and  the  corresponding 
simplified  design  and  programming  problem. 

3.  Control  Schemes 

Robot  arm  control  maintains  the  dynamic  response  of  a 
computer-based  manipulator  in  accordance  with  some  predetermined 
system  performance  goal.  With  a  valid  model,  simulation  furnishes  a 
testing  technique  for  control  strategies  without  the  expense  and 
mechanical  problems  of  working  with  actual  manipulators.  These 
techniques  can  also  include  the  off-line  programming  and  testing  of 
suitable  optimal  and  sub-optimal  control  schemes.  Control  laws  can 
be  evaluated  numerically  so  that  dynamic  effects  can  be  resolved 
without  inflicting  damage-  or  tying  up  an  actual  manipulator  system. 

4.  Robot  Teaching  and  Workspace  Layout 

Robot  arm  simulation  allows  development  of  robot  planning  and 
programming  and  can  provide  significant  time  savings  through  robot 
physical  modelling  and  workspace  layout. 

C.   THE  ROBOT  MODELLING  AND  SIMULATION  PROBLEM 

The  robot  arm  dynamic  modelling  problem  is  one  which  requires  the 
formulation  of  an  accurate  and  efficient  mathematical  representation 
of  all  components  involved  in  manipulator  dynamics.  Effective 
mathematical  formulations  are  used  to  predict  the  amount  of  actuator 
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torque  required  to  produce  a  specific  motion  or  they  are  used  to 
predict  the  motion  given  the  input  torque.  In  this  work  the  torque 
on  the  arm  joints  was  assumed  as  a  known  function  of  time. 


V 

e 
ea 

a 

'  > 

Trajectory 
Design 

Control 

Dynamic 

Integrators 

ea 

System 

T 

a 

Model 

•  • 

e 

m 

• 

e 

m 
Figure  1.   Manipulator  Model  Block  Diagram 

A  reasonably  complete  version  of  an  entire  manipulator  model 
includes  a  control  system,  as  shown  in  Figure  1.  Here  the  actual  arm 
dynamics  are  replaced  by  a  mathematical  dynamic  model  of  the 
manipulator.   Model  accuracy  is  determined  by  comparing  modelled 

a  ■  • 

trajectory  information  (6  ,0  )  against  actual  velocities  (6  )  and 

mm  a 

positions  (6  ) . 
a 

This  thesis  does  not  consider  the  mathematical  formulation  of 
trajectory  design,  control,  or  feedback  systems,  as  shown  in  Figure 
1.  It  does  combine  a  previously  developed  mathematical  formulation 
with  a  method  of  modelling  robot  arm  movements  developed  by  Walker 
and  Orin  [Ref.  3].  A  simplified  robot  arm  simulation  problem  can  be 
stated  as:  given  a  manipulator  dynamic  model,  it's  link  mass  and 
inertia  properties,  it's  initial  link  alignments,  and  the  joint 
force/torque  signal  and  limits,  then  predict  the  motion  of  each  joint 
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and,  ultimately,  the  end-effector  (i.e.   individual  displacements, 

velocities,  and  accelerations  versus  time) ,   The  simulation  is  not 

updated  with  actual  velocity  and  position  data,  as  is  the  model  of 

Figure  1,  but  runs  on  its  own  predictions,  as  shown  in  Figure  2. 

Simulation  accuracy  is  evaluated  by  comparing  predicted  values  (6  ) 

to  actual  values  (8  ). 

a 


Dynamic 

^ 

Integrator 

,-*  es 

Model 

.  . 

6 
s 

^  6S 
s 

; 

k 

Figure  2.   Manipulator  Simulation  Block  Diagram 

1.   Current  Solution  Methods 

Many  methods  of  accomplishing  computer  simulation  of 
complicated  manipulator  mechanisms  have  been  developed  recently.  An 
examination  of  the  more  prominent  simulation  techniques  was  made  and 
a  summary  of  these  follows. 

Vereshchagin   developed   a   method   which   uses   a   direct 
implementation  of  Gauss's  principle  of  least  constraint  [Ref.  4]. 
Nelson  and  Chang  explored  nonlinear  motor  current  and  voltage  limit 
effects  when  simulating  a  specific  robot  arm  having  three  Cartesian 
axes  of  motion  with  two  rotational  wrist  axes  [Ref.  5], 
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Murray/Neuman  and  Cesareo/Nicolo  have  developed  a  computer 
program  which  can  symbolically  generate  the  dynamic  equations  of 
motion  [Ref.  6,7]. 

Meyer/Jayaraman,  Leu/Mahajan,  Wang/Lin,  and  Staufer  showed 
the  practical  use  of  graphics  in  simulation  software  [Ref. 
8,9,10,11].  Backhouse  described  a  dynamic  simulation  which  was 
implemented  on  an  analog  computer  and  included  the  modelling  of 
hydraulic  motors  and  servo-valves  [Ref.  12]. 

Brandeberry/Dufour/Spalding  have  formulated  specific  dynamic 
equations  for  a  small  robotic  arm  they  are  designing  [Ref.  13]. 
Cvetkovic/Vukobratovic  developed  dynamic  modelling  using  the  Newton— 
Euler  formulation  with  recurrence  relations  for  velocities, 
accelerations,  and  forces  [Ref.  14].  Derby  has  considered  simulation 
using  cam-motion  profile  techniques  with  the  General  Robot  Arm 
Simulation  Program  (GRASP)  [Ref.  15]. 
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II.   BASIC  MANIPULATOR  CONCEPTS 

A.  MANIPULATOR  ARM  CONFIGURATION 

An  understanding  of  manipulator  kinematics,  statics,  and  dynamics 
is  fundamental  to  designing  a  simulation  system.  The  Robot 
manipulators  considered  in  this  thesis  are  composed  of  serially 
connected,  rigid  linked  structures,  jointed  by  one-degree  of  freedom 
joints.  Manipulator  joints  which  rotate  about  an  axis  are  called 
revolute  and  those  which  change  their  position  along  an  axis  are 
called  prismatic.  The  principal  degree  of  freedom  for  revoluate 
joints  is  the  joint  angle,  and  for  prismatic  joints  is  the  translated 
distance.  At  the  end  of  a  manipulator  chain,  there  is  usually  some 
tool  or  hand/gripper  mechanism  which  performs  the  actual  task  for 
which  the  robot  was  designed.  Due  to  their  variety  and  complexity, 
the  devices  placed  in  this  position  are  designated  as  end-effectors. 
Joint  actuators  are  typically  located  between  adjacent  links  to  allow 
proper  joint  positioning. 

B.  KINEMATICS 

The  basic  robotics  problem  is  to  perform  a  task.  Motion  planning 
is  needed  to  map  the  composite  motions  required  for  each  overall 
task.  In  planning  manipulator  movement,  the  primary  task  is  to 
achieve  a  specified  end-effector  Cartesian  position  with  respect  to 
the  robot's  environment.  A  kinematic  evaluation  is  used  to  represent 
the  positional  relationships  of  the  manipulator  mechanism. 
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1.  The  Direct  Kinematics  Problem 

In  direct  kinematics,  a  joint  coordinate  system  is  translated 
into  a  fixed  coordinate  system.  Fixed  or  ''world*'  coordinate 
systems  must  be  established  and  the  origin  can  be  chosen  at  any 
location,  including  the  foundation  of  a  conveyor  belt,  a  central 
geographical  location,  or  even  the  base  of  the  robot.  For  the 
purposes  of  this  thesis,  the  robot  base  will  be  used  as  the  fixed 
coordinate  system  origin.  Further  details  concerning  this  are 
provided  in  section  II.A.3.b  which  follows.  The  direct  kinematics 
problem  is:  given  the  robot  arm's  joint  angle  vector,  find  the 
manipulator's  end  effector  position  and  orientation  with  respect  to 
the  fixed  coordinate  system.  This  problem  has  an  analytically  unique 
solution.  [Ref.  16] 

2.  The  Inverse  Kinematics  Problem 

The  inverse  kinematics  problem  can  be  stated  as:  given  the 
position  and  orientation  of  the  end  effector  with  respect  to  the 
fixed  coordinate  system,  find  the  robot  arm  joint  angles.  This 
problem  does  not  have  a  unique  solution  due  to  the  transcendental 
nature  of  the  kinematic  equations.  [Ref.  16,17,18] 

3.  Link  Representations 

A  link  coordinate  frame  (attached  to  the  body)  is  shown  in 
Figure  3.  A  summary  of  the  link  numbering  convention  and  specific 
procedural  steps  for  establishing  link  axes  and  defining  link 
parameters  are  provided  in  Appendix  A. 
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•  JOINT  i  +  2 
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Figure  3.   Link  Coordinate  Convention 

An  essential  part  of  the  kinematics  problem  is  to  find  a 
transformation  matrix  that  relates  the  body-attached  link  coordinate 
frame  to  the  fixed  coordinate  frame.  General  solution  methods  for 
the  kinematic  problem  have  been  proposed  by  several  researchers  [Ref. 
18,19,20,21]  and  summarized  below. 

a.   Denavit-Hartenberg  Convention. 

Denavit  and  Hartenberg  were  the  first  to  relate  spatial 
configurations  between  neighboring  robot  arm  links  using  a  matrix 
representation  of  rigid  link  kinematics  [Ref.  22,23],  Uicker 
extended  these  methods  to  a  generalized  displacement  analysis  [Ref. 
22,24].   Under  Denavit  and  Hartenberg' s  well  developed  and  popular 
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convention,  the  kinematics  solution  for  each  link's  position  and 
orientation  is  founded  upon  a  4x4  homogenous  link  transformation 
matrix.  The  individual  Denavit-Hartenberg  link  coordinate  matrix,  or 
A  matrix,  is  a  4x4  transformation  matrix  from  link  i's  coordinate 
system  to  link  (i-l)'s  coordinate  system  as  shown  in  Figure  3.  The 
A.  matrix  describes  the  first  link's  position  and  orientation 
relative  to  the  robot  base.  A.  describes  the  second  link's  position 
and  orientation  relative  to  the  first  link,  etc.  The  A  matrix  is 
expressed  as  [Ref .  22] : 
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i      i 
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l 

d. 

l 

1 
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and  A  =  I,  the  identity  matrix, 
o 

The  coordinate  system  for  the  manipulator  used  in  this 
thesis,  the  PUMA  arm,  is  shown  in  Figure  4. 
b.   Transformation  Matrices. 

Once  an  orientation  for  each  link  is  established  by  it's 
link  transformation  matrix,  than  any  link  position  can  be  established 
in  fixed  coordinates.  For  example,  transformation  matrices  are  used 
to  map  the  link  6  (end-effector)  position  and  orientation  vectors 
into  the  robots  base  coordinate  system,  as  shown  in  Figure  5.   In 
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Figure  4.   PUMA  Arm  Link  Coordinate  System 
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general,  the  transformation  matrix,  T  ,  is  obtained  by  chain 
multiplying  successive  link  coordinate  transformation  matrices,  A., 


or: 


T1  =  n  A.  ;  for  i=l,2...n 
°    j=l  J 


(2) 


For  example:    Tq  =  Tg  =  A^A^A^ 


Figure  5.   Position  and  Orientation  Vector  of  End-effector 
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The  transformation  matrix  is  comprised  of  four  submatrices  as  follows 
[Ref.  19,20] : 


o  = 


n  o  a  I  p 

0  0  0     1 


Orientation  Vectors     Position  Vector 


.  Perspective  Transform   Scaling  Factor 


i  _  I  n  o  a  p    m 
o  =  L0001J  ~ 


n  o  a  p 

X   X   X   X 

n  o  a  p 
ny  oy  ay  py 
L  0Z  0Z  0Z  1Z 


(3) 


(4) 


where : 


n  =  end-effector  normal  vector.  (Orthogonal  to  robot  arm  fingers, 
with  a  parallel  jaw  hand.) 

o  =  end-effector  orientation  vector.   Points  in  finger  motion 
direction. 

a  =  end-effector  approach  vector.   Points  in  direction  normal  to 
hand  base. 

p  =  end-effector  position  vector.   Points  from  robot  base 
coordinate  system  to  origin  of  hand  coordinate  system. 

c.   Position  vectors. 

T 
A  position  vector  r  can  be  represented  as  (r  ,r  ,r  ,1)  . 

x  y  z 

The  '1'  allows  rotation  and  translation  combinations  when  applying 
transformations.   This  is  done  by  multiplying  a  position  vector  by  a 
link  coordinate  matrix,  as  follows: 
Let   r  =  vector  from  link  i's  origin  to  a  point, 

expressed  in  the  link  i  coordinate  system, 
r  =  vector  from  link  (i-l)'s  origin  to  the  above  point, 

expressed  in  the  link  (i-1)  coordinate  system. 
Therefore: 


i-1 


r  =  A.   r. 

i 


(5) 
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Similarly,   a  vector  can  be   defined  from  the   i 

coordinate  system  to  a  fixed  point  in  link  j,  but  expressed  in  link  i 

i  T 

coordinates  as   r.  =  (r  ,r  ,r  ,1)  .   Therefore,  points  in  adjacent 

j      x  y  z 


coordinate  systems  are  related  by: 


i-1     .   i 
r.  =  A.   r, . 
J    i   J 


(6) 


Thus,  any  positions  in  two  different  coordinate  systems  i  and  j  can 
be  related  by  cascading  the  transformations: 


k     j    k 

with  T^"  as  defined  in  Equation  (2).   The  matrix  r!  =  I.   The 
J  J 

superscript,  0,  is  omitted  when  referring  to  the  base  coordinate 
system,  thus  it  can  be  written  r,  =  r,  and  T.  =  T. .   Therefore, 

r,  =  T.  jr, 

k    j   k 

d.   Euler  Rotation  Transformation  Conventions. 


(7) 


(8) 


In  addition  to  the  above,  robot  arm  link  orientations  can 
also  be  expressed  by  a  sequence  of  rotations  about  the  x,  y,  z  axes. 
These  rotations  can  be  in  the  form  of  Euler  angles: 


Euler(f,6,9)  =  Rotated,  1),  Rotate(y,6),  Rotate(z.f) 


(9) 


9  about  the  z  axis 

6  about  the  new  y  axis 

$  about  the  new  z  axis 

The  Euler  homogeneous  tranformation  matrix  would  then  be 


R(l,6,«)  I  p 


0    0    0 


Rotation     Position  Vector 
I 


Perspective   Scaling  Factor 


(10) 
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C.   TRAJECTORIES 

Trajectories  consist  of  the  time  function  of  the  positions, 
velocities,  and  accelerations  of  points  on  the  arm.  As  previously 
stated,  to  move  a  robot  manipulator  end-effector  to  a  point  in 
Cartesian  space  the  individual  link  joint  angles  must  be  found.  The 
task  of  moving  the  end-effector  from  one  place  to  another  in  a  timed 
sequence,  requires  the  formulation  of  a  trajectory  plan.  This  can  be 
done  by  converting  desired  end-effector  movements  into  time- 
serialized  movements  for  all  of  the  manipulator  joints.  Once 
formulated,  each  robot  arm  joint  will  move  through  the  sequence 
trajectory  points. 

The  specific  values  for  position,  velocity,  and  orientation 
depend  upon  the  type  of  trajectory  to  be  followed  between  workpoints. 
Various  methods  have  been  developed  to  determine  trajectory  plans 
[Ref.  19,25,26,27].  For  example,  a  common  class  of  trajectories 
corresponds  to  the  straight  line  movement  of  the  end-effector  between 
workpoints.  Additionally,  trajectory  plans  can  depend  upon  whether 
continuous  (smooth)  or  point-to-point  motion  is  desired.  Trajectory 
planning  must  not  produce  trajectories  which  exceed  actuator  bounds 
or  arm  stress  limits.  However,  the  trajectory  planning  process  is 
seen  to  be  beyond  the  scope  of  this  thesis.  Trajectory  values  used 
to  verify  simulation  and  modelling  results  were  obtained  from 
previously  published  data  for  the  specified  manipulator  arm. 
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To  move  a  manipulator  along  a  trajectory  in  accordance  with  the 
prescribed  plan,  torques  must  be  exerted  by  joint  actuators.  The 
forces/torques  required  must  be  computed.  The  relationship  between 
kinematic  states  and  driving  torques  is  seen  during  kinetics 
computations. 

D.   KINETICS 

System  dynamics,  which  determine  the  effects  of  torque  on  the 
manipulator,  are  complex  due  to  multiple  degrees-of-freedom  and 
complicated  interlink  relationships.  Of  principal  concern  in  this 
thesis  are  those  internal  forces  and  moments  generated  as  a  result  of 
torque  application  and  subsequent  motion.  Forces  generated  by 
payload  and  obstacles  are  not  modelled. 

1.  Statics 

Static  forces  exerted  by  the  manipulator  on  the  environment 
and  static  link  forces  should  be  evaluated  in  robot  modelling.  An 
equilibrium  analysis  of  the  manipulator  system  at  rest  is  performed 
by  equating  all  external  forces  acting  on  the  robot  to  zero  and 
computing  the  corresponding  internal  forces.  A  static  force 
evaluation  was  not  done  for  this  thesis. 

2.  DVAlembert's  Principle 

Significant  contributions  to  theoretical  dynamics  were  made 
during  the  eighteenth  century  by  J.R.  d'Alembert  (1717-83),  L.  Euler 
(1701-83),  and  J.L.  Lagrange  (1736-1813).  D'Alembert's  principle 
describes  a  viewpoint  where   a  body's   acceleration  generates  an 
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inertia  force  which  is  considered  with  the  other  forces  acting  on  the 
body  [Ref.  28],   This  concept  of  dynamics,  with  an  inertia  force 
formulation,  creates  an  artificial  state  of  ''dynamic*'  equilibrium. 
The  external  forces  acting  on  the  manipulator  are  summed  and  equated 
to  ' 'd'Alembert' '  forces. 

3.  Dynamic  Equations  of  Motion 

Manipulator  dynamic  equations  relate  forces/torques  to  joint 
angle  positions,  velocities,  and  accelerations  by  taking  into  account 
link  masses  and  geometic  properties.  These  equations  can  be 
formulated  and  solved  using  one  of  several  approaches.  For  example, 
one  or  two  link  mechanisms  can  be  formulated  directly  from  proper 
free-body  diagrams  [Ref.  31],  With  simple  mechanisms,  the  entire 
free-body  system  can  be  described  in  a  framework  of  ''inertial 
coordinates''.  However,  for  more  complex  systems,  Lagrangian, 
virtual  work  [Ref.  32],  or  Newton-Euler  [Ref.  19,21,33]  formulations 
must  be  used. 

4.  The  Dynamic  Simulation  Problem 

The  dynamic  robot  arm  modelling  and  simulation  problem  is 
restated  as  follows:  Given  a  manipulator  dynamic  model,  it's  link 
mass  and  inertia  properties,  it's  initial  configuration,  and  joint 
torque  signals;  predict  the  joint  positions,  velocities,  and 
accelerations  which  are  be  produced  at  each  joint  and  the  end- 
effector.  (In  the  inverse  manipulator  dynamics  problem,  which  forms 
the   heart   of   control   programming   efforts,   joint   torques   are 
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determined  from  the  relationships  of  joint  positions,  velocities,  and 
accelerations.) 

a.   Dynamic  Equation  Simplification. 

Torque  signals  must  be  quickly  and  accurately  computed  to 
provide  a  manipulator  with  proper  control  power  to  carry  loads  along 
planned  paths.  In  order  to  accomplish  this  real-time  control,  they 
must  be  computed  on  the  order  of  at  least  60  times  per  second  [Ref. 
27].  Therefore,  there  has  been  strong  pressure  to  increase 
computational  efficiency,  allowing  dynamic  equation  solution  in  real- 
time control  systems.  [Ref.  34,35].  Additionally,  as  mechanical 
design  and  control  engineers  use  dynamic  analysis  to  develop  better 
manipulators,  there  will  be  an  increased  need  to  further  decrease 
computer  simulation  time  and  costs. 

Many  analytical  schemes  have  been  proposed  to  make  the 
real-time  dynamics  predictions  computationally  feasible.  Most 
significant  are  dynamic  simplifications  which  ignore  some  terms  and 
correct  errors  with  feedback  [Ref.  21].  A  good  understanding  of 
terms  and  their  significance  must  be  developed  to  allow 
simplification  of  the  dynamic  equations  of  motion  in  controllers. 

The  most  common  method  of  simplifying  dynamics  has  been 
to  ignore  Coriolis  and  centrifugal  forces,  which  represent  the 
greatest  computational  burden  in  dynamics  calculation  [Ref.  21]. 
Other  assumptions  include  the  consideration  of  some  system  elements 
as  massless  and  the  neglect  of  certain  joint  offsets  [Ref.  19j  page 
31]. 
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Results  obtained  in  simplification  are  typically  valid 
only  in  specific  ranges  of  operation.  Coriolis  and  centrifugal 
forces  cannot  be  ignored  at  high  movement  speeds.  For  example,  it  is 
not  a  good  approximation  to  neglect  the  velocity  terms  except  at  the 
beginning  and  end  of  the  arm  motion  [Ref.  33,36]. 

Another  simplification  includes  the  ignoring  of  actuator 
dynamics.  When  actuator  dynamics  are  considered  in  manipulator 
dynamics,  higher-order  differential  systems  result.  This  high  order 
system  is  not  readily  solvable  [Ref.  29;  page  71].  Therefore, 
dynamic  analysis  is  greatly  simplified  if  actuator  and  rigid 
manipulator  link  dynamics  are  separated  in  the  simulation  approach. 
The  effects  of  actuator  dynamics  is  considered  beyond  the  scope  of 
this  thesis. 

Computational  reductions  using  model  reformulations  have 
also  been  presented  which  use  various  matrix,  vector  and  numeric 
analysis  techniques.  These  methods  are  discussed  in  the  sections 
which  follow. 

E.   THE  LAGRANGE  FORMULATION 

The  first  manipulator  dynamics  formulations  were  based  on 
Lagrange  equation  derivations  [Ref.  29]  and  were  so  inefficient  that 
torques  for  specific  trajectories  could  not  be  computed  in  anything 
close  to  real-time  requirements.  Although  this  is  a  serious  problem 
in  manipulator  control,  Lagrange  equations  were  one  of  the  principal 
methods  of  simulation  and  analysis  for  almost  a  decade  [Ref.  29]. 
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Through  persistent  efforts  to  increase  computational  efficiency, 
Lagrange  equations  now  provide  the  foundation  for  current,  more 
effective  real-time  control  methods.  Discussions  of  Lagrangian 
equations  can  be  found  in  most  dynamics  textbooks  [i.e.  Ref.  37]. 
The  Lagrange  formulation  can  represent  the  dynamic  behavior  of  rigid 
body  systems.  Kahn  was  the  first  to  apply  a  general  formulation  of 
the  Lagrange  equation  specifically  to  robot  manipulators  [Ref.  38]. 
Uicker  adapted  Kahn's  work  for  open  chain  manipulator  linkages,  using 
homogeneous  coordinates  and  the  transformation  matrices  discussed 
earlier  [Ref.  24]. 

The  chief  advantage  of  Lagrange  formulated  manipulator  equations 
is  in  the  straightforward  and  simple  methodology,  since  the  six 
manipulator  links  are  each  consecutively  referenced  to  the  preceeding 
link.  Lagrangian  generalized  coordinates  thus  lead  to  a  systematic 
representation  of  multilink  mechanisms. 

1.   Energy  Calculations 

The  Lagrange  manipulator  model  formulation  begins  with  the 
formulation  of  kinetic  energy  (KE)  and  potential  energy  (PE) 
expressions  [Ref.  24]. 

a.   Kinetic  Energy. 

The  critical  difference  between  the  Lagrangian-Euler  and 
Newton-Euler  methods  comes  in  the  method  used  to  formulate  kinetic 
energy.  As  previously  stated  in  equation  (8),  a  position  vector  to  a 
point  (for  example  a  differential  mass)  in  base  coordinates,  with 
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respect  to  link  j  when  i=0  (for  the  base)  can  be  written  r,  = 
or  more  simply,  r  =  T.   r.   Velocity  can  then  be  written: 


v  = 


dr 
dt 


1  rK 


L  k=l  3qk 


T.  Jr. 


(11) 


and 


r2  - 
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=  r  '  r  =   tr  (r  r  ) 
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where: 


v*  =  tr 


5     ^  iT   rj  (rj)' 
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L  k=l  p=l 


3T. 


a  4. 


MP 


(13) 


q.  =  joint  variables,  the  generalized  coordinates  signifies  the 
joint  angle,  6.,  for  single  degree-of-freedom  rotary  joints 
and  displacement  of  sliding  joints.   (Multiple  degree-of- 
freedom  joints  are  modeled  as  single  degree-of-freedom  joints 
with  intermediate  links  of  zero  length  and  mass.) 


Since  kinetic  energy  =  Va  mva,  then  the  kinetic  energy 

j 


dKE.  of  a  differential  mass  at  point   r  is: 

dKE.  =  J  tr  (jr(jr.)T)  dm 
J    2  j 


(14) 


k=l  p=l 


r  3T. 
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(15) 


In  the  above,  a  trace  operator  forms  the  tensor  product   r  r  from 

which  the  inertia  matrix  J.  is  found. 

J 

The  total  kinetic  energy  KE.  of  a  link  is  found  by 
integrating  over  all  the  differental  masses  in  all  the  links  and 
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summing  each  link's  kinetic  energy  KE .  with  respect  to  the  inertial 

•J 


frame : 


KE, 


=  \   \   dKE.   =  J  tr  (  T.  /  (jr  jrT)  dm  iT  ) 


"  I  tr  (  ^  J.  i^  ) 


(16) 
(17) 


where : 


J.  =  /(  r   r  )  dm  =  the  inertia  matrix 

■I 


(18) 


J.  =  inertia  tensor  with  respect  to  the  joint  situated  next 
to  link  j  expressed  in  j's  coordinates 
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k. ..  =  radius  of  gyration  of  link  j  about  the  j-k  axes 
m  =  link  j  mass 

T.  =  transformation  matrices  representing  both  linear  and 
angular  link  velocities. 

Therefore,  the  manipulator  structure's  kinetic  energy  is: 
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The  actuator  kinetic  energy  at  the  joints  is: 

K  =  ~   I  a.  4?  (21) 

actuator.    2    j  nj 
J 

Summarizing,  the  total  kinetic  energy  is: 


KE 

i=l  j=l  k=l  i=l 


(22) 


b.   Potential  Energy. 

The  potential  energy  of  a  individual  link  can  be 

expressed  as  [Ref .  19,21]  : 

PE  -  mgh  =  -m  g  .  r  (23) 

where 

r  =  position  of  the  link's  center  of  mass 
g  =  the  gravity  vector     Igl  =  9.8062  m/s* 
m  -  mass 
h  =  height 

For  a  link  with  a  mass  center  at  a  position  vector  r. 

J 

with  respect  to  link  j  coordinate  frame  T.  is: 

J 

PE  -  -m.  gT  T.  jr.  (24) 

where:  J     J   J 

r.  =  a  vector  from  link  j  origin  to  its  center  of  gravity 

m.  =  the  link  j  mass 
J 

The  total  robot  arm  potential  energy  is  the  sum  of  each 
link's  potential  energy  PE.  expressed  in  the  base  coordinate  frame: 


n  n 

f  AT>U  =     \     —  «•    a       T       3~ 

(25) 


PE  =  5  f   dPE.    =   3   "  m.  gT  T.  jr. 
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2.   Lagrange  Equation  Formulation 

After  the  Lagrangian,  L  =  KE  -  PE,  is  established  it  is 
applied  to  the  Lagrange  manipulator  formulation  to  produce  the 
generalized  force  required  for  joint  i  to  drive  the  ith  robot  arm 
link.   Symbolically,  we  can  define  the  forces  as: 


d 

r    3l    i 

3L 

f .    =  — 
1        dt 

.  di  . 

3q 

i— 1, * . . ,  n 


(26) 


The  dynamic  equations  of  motion  for  a  n  link  manipulator  as 
derived  for  the  formulation  of  the  Lagrangian  function  are  [Ref .  30] : 
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This  dynamics  equation  is  a  closed  form  expression,  and  shows 
that  an  applied  joint  torque  is  explicitly  dependent  on  all  joint 
movements.     The   equation's   complexity   comes   from   the   dynamic 
interactions  between  each  of  the  manipulator  links. 

For  purposes  of  the  following  discussion,  it  is  important  to 
recognize  that  there  are  three  types  of  terms  in  the  Lagrangian 
dynamic  equation.   They  are: 

(1)  inertial  forces  which  are  proportional  to  the  joint 

accelerations  &i 
k 
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(2)  velocity  forces  which  are  proportional  to  the  product  q  q 

a.  centripetal  forces  when  k  =  p  and  proportional  to  q2 

b.  Coriolis  forces  when  k  f   p. 

(3)  gravity  forces 

3.  Lagrange  Matrix  Formulation 

The  manipulator  arm  dynamics  of  equation  (27)  can  be 
rewritten  in  matrix-vector  form  [Ref.  6,39]: 
n  n   n 

fi  "  1     »ij  4j   +  }        }      C..,  qj4k   +   G.  (2g) 

j=l  j=l  k=l 

or,  more  compactly  (adding  the  effects  of  outside  forces): 

F(t)  =  H(q)q'  +  C(q.q)q  +  G(q)  +  K(6)Tk  (29) 

In  the  Lagrangian  formulation,  the  dynamic  model  parameters  for  the 
above  are  [Ref.  3,6,20]: 

H(q)  =  Inertial  coefficient  (n  x  n)  matrix 

C(q,q)  =  Centrifugal  and  Coriolis  force  n-vector 

G(q)  =  Gravitational  force  nr-vector 

K(q)  =  Jacobian  matrix  of  joint  forces  created  due  to 
external  forces  exerted  on  link  n 

k  =  n  vector  of  external  force  exerted  on  link  n 

4.  Limitations  in  the  Lagrange  Formulation 

The  dynamics  computation  using  the  Uicker  Lagrangian 
formulation,  equation  (27),  is  far  too  slow  for  use  in  real-time 
control  or  in  simulation.   Luh,  Walker,  and  Paul  found  that  7.9 
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seconds  was  required  for  each  evaluation  of  the  Dicker  Lagrangian 
[Ref.  27]  for  a  six  link  manipulator.  As  can  be  seen  from  equation 
(27),  a  triple  summation  is  evaluated  for  each  joint  torque  to  obtain 
the  velocity  product  terms.  Although  it  can  be  argued  that  the 
velocity  products  can  be  neglected  [Ref.  19,21],  another 
computational  inefficiency  exists  in  the  double  summations  that  must 
be  computed  at  each  joint  for  the  acceleration  terms.  The  double 
sums  are:  (1)  the  inside  bracket  summation,  ranging  from  joints  1  to 

j,  and  (2)  the  outside  bracket  summation,  ranging  from  joints  i  to  n. 

4 
This  results  in  computational  operations  proportional  to  n   when  a 

triple   and   double   summation   is   considered   with   joint   torque 

computation  for  n  joints.   Therefore,  reformulation  is  necessary  to 

satisfy  real  time  control  requirements. 

F.   RECURSIVE  LAGRANGIAN  DYNAMICS 

In  1981,  Hollerbach  substantially  reduced  the  Lagrangian  method's 
computational  requirements  by  establishing  a  simple,  succinct 
formulation  of  (forward)  link  numbering  recurrence  relationships 
[Ref.  40].  Hollerbach's  reformulations  decreased  the  number  of 
computational  operations  to  the  order  of  n.  Additionally,  Waters 
found  generalized  forces  could  be  expressed  in  a  manner  that  that 
took  advantage  of  backward  link  numbering  recurrence  relations  [Ref. 
29], 

The  structure  of  forward  and  backward  recursions  contributes 
greatly  to  increased  computational  efficiency.   Each  approach  yields 
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equivalent  equations,  through  (1)  a  backward  recursive  computation  of 
the  linear  and  angular  velocities  and  accelerations  from  the  base  to 
the  manipulator  end,  and  (2)  a  forward  recursive  computation  of 
generalized  working  forces  and  torques  from  end  to  base.  Recursive 
dynamic  equations  can  compute  angular  velocity,  linear  and  angular 
acceleration,  and  forces  and  torques,  and  achieve  great  efficiency 
because  they  only  execute  recursions  once,  thus  avoiding  costly 
matrix  summations. 

Hollerbach's  reformulation  also  showed  that  the  number  of 
coefficients  can  be  cut  in  half  by  using  3x3  transformation  matrices 
and  explicit  position  translations  insteads  of  the  4x4  homogeneous 
transformations  discussed  earlier.  There  are  many  inefficiencies  in 
4x4  matrices  due  to  the  many  zeros,  ones,  and  redundancies  in 
original  A.  matrices.  For  purposes  of  this  thesis,  however,  4x4 
matrices  are  used.  Greater  computational  efficiency  has  been 
sacrificed  here  for  greater  flexibility,  and  compatibility  with  many 
published  research  articles. 

As  stated  in  section  II. 5. b,  the  Lagrange  formulated  equation  for 
obtaining  generalized  forces  for  an  n-link  manipulator  is: 
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Hollerbach  showed  that  to  obtain  generalized  forces,  the 
computational  procedure  requires: 

(1)  computation  and  storage  of  the  T7  terms 

■I 

(2)  computation  and  storage  the  9T./3q,  and  32T./3q,3q,  terms 

j    k        j    k  1 

1.   Lagrangian  Dynamics  with  Backward  Recursion 
The  derivation  of  the  generalized  forces  is: 
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where: 


tj  =  Vi  Ai 
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(33) 
(34) 

(35) 


The  initial  conditions  for  the  base,  the  linear  and  angular 

•       •  ■ 

velocities  and  accelerations  T  and  T  ,  are  zero,  since  the 

o      o 

manipulator  base  does  not  move. 
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2.   Forward  Recursive  Lagrangian  Dynamics 

The  next  step  in  the  recursion  formulation  clarifies  how 
forces/torques  are  reflected  back  from  manipulator  tip  to  base.   This 
forward  recursion  leads  to  increased  Lagrangian  dynamics  efficiency. 


Since:    T.  s  A.  ,,  A.  .-, . .  .A.    and 

j     l+l  i+2    j 


3T. 
J  _ 


3T. 


9<1i   dqi  Tj 


The  genralized  force  equation  (6)  becomes: 
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where: 
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Substituting  the  D.  and  c.  terms  into  equation  (36),  the  forces  are 


fi=  tr 
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T8Ti 
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(39) 


This  recursive  formulation  is  computed  by: 


(1)  computing  T.  terms  from  i=l  to  i=n  using  equation  (39) 

(2)  computing  D  and  c  terms  from  i=n  to  1=1. 
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The  second  step  is  where  computational  complexity  of  an  n 

4 
link  arm  is  reduced  from  the  order  of  n   to  n.   The  only  way  to 

further  reduce  this  linear  complexity  would  be  to  reduce  the  number 

of  linear  polynomial  coefficients,  which  will  not  be  done  for  the 

generalized  program  used  in  this  thesis. 

Although  a  recursive  Newton-Euler  formulation  is  almost  three 

times  more  efficient  that  a  recursive  Lagrangian  formulation  [Ref. 

40],  there  appears  to  be  no  fundamental  difference  between  the  two 

formulations  when  properly  configured  kinematically   [Ref.   41]. 

Although  not  as  efficient,   the  Lagrangian  formulation  has  been 

selected   here   because   it   offers   straightforward   programming 

algorithms   which   do   not    depend    on    specific    manipulator 

configurations. 


44 


III.   MOTION  MODELLING  AND  SIMULATION 

A.   MODELLING  AND  SIMULATION  APPROACH 

The  modelling  approach  in  this  thesis  is  a  Lagrangian  description 
of  a  manipulator  arm's  dynamic  behavior.  The  model  computer  program 
utilizes  a  general-purpose  kinematic  analysis  algorithm  which 
includes  both  forward  and  backward  recursive  Lagrangian  solutions. 
These  algorithms  are  based  on  coordinate  frame  representations  for 
the  links  and  formal  Lagrangian  mechanics  interpretations.  The  use 
of  generalized  algorithms  allows  future  evaluation  of  many  different 
dynamic  models  within  one  common  software  program  using  a  data  base 
of  various  arm  parameters.  Additionally,  a  generalized  program  can 
be  simplified  for  specific  manipulator  configurations. 

The  individual  control  torques  used  in  this  program  are  provided 
in  the  form  of  input  data.  A  given  control  force  is  used  to  compare 
the  simulated  dynamic  response  against  a  known  experimental  response. 

For  each  time  step,  two  tasks  are  involved  in  the  simulation:  (1) 
the  robot  arm  trajectory  is  determined  through  integration  of 
acceleration  and  velocity  for  assumption  of  constant  acceleration, 
and  (2)  the  joint  accelerations  for  the  next  time  step  are  obtained 
through  solution  of  the  dynamic  equations  of  motion. 
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B.   COMPUTATION  CONSIDERATIONS 

1.  Simplifying  Assumptions 

Because  of  equation  complexity,  past  researchers  have  used 
simplifying  assumptions  for  the  mechanical  model  so  that  a  solution 
could  be  obtained  [Ref.  29;  page  62].  This  program  includes  all 
terms  which  appear  in  the  Lagrangian  formulations  as  shown  in 
equations  (27)  and  (32). 

2.  Computational  Efficiency 

Current  research  emphasizes  computational  efficiency  to  allow 
a  real-time  dynamic  equations  solution  for  control  schemes  [Ref.  27] . 
Additionally,  increased  computational  efficiency  promotes  simulation 
cost  optimization.  The  emphasis  of  this  program  is  to  provide  the 
foundation  for  future  control  work  and  to  offer  as  general  a  model  as 
possible.  Therefore,  the  program  was  not  designed  as  the  most 
computationally  efficient,  although  efficient  software  schemes  were 
used  where  possible.  Hollerbach  has  concluded  that  4,388 
multiplications  and  3586  additions  are  required  for  the  formulation 
selected  for  use  in  this  program.  Reductions  to  2,195 
multiplications  and  1,719  additions  could  be  made  using  3x3 
transformation  matrices  [Ref.  40]. 

3.  Structure  Flexibility 

All  manipulators  are  flexible  to  some  extent.  However,  the 
rigid-body  dynamics  presented  here  are  an  idealization  for  a  real 
manipulator.  [Ref.  43] 
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4.  Integration 

Currently,  the  standard  procedure  to  solve  a  nonlinear  system 
is  to  use  an  iterative  numerical  time  integration  method  on  the  set 
of  nonlinear  differential  equations  describing  the  system  [Ref.  44]. 
The  thesis  program  as  currently  designed  will  analyze  a  rigid  link 
manipulator  using  a  trapezoidal  numerical  time  integration  algorithm 
[Ref.  45],  Through  the  assumption  of  constant  joint  acceleration 
during  time  steps,  no  iteration  is  necessary. 

5.  Frictional  Forces 

Frictionless   conditions   were   assumed   for   the   thesis 
simulation. 

6.  Vibration  and  Dynamic  Stabiltv 

Vibrational  and  system  dynamic  stability  effect  were  not 
considered  in  this  thesis  work. 

C.   PROGRAM  APPROACH 

The  FORTRAN  77  language  was  chosen  to  implement  the  dynamic 
formulation.   The  source  code  produced  for  this  program  was  complied 
on  an  IBM  3033  computer  using  the  VS  FORTRAN  compiler  (Release  3.0). 
The  methodology  and  programming  philosophy  used  in  generating  the 
source  code  are  provided  in  the  sections  which  follow. 

1.   Principal  Program  Matrices 

In   robot   arm   modeling,   five   terms   (inertia,   Coriolis, 
centrifugal,  gravity,  and  external)  make  up  the  joint  forces  or 
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moments.   Equation  (29)  can  be  rewritten  to  model  the  dynamics  of  a 
revolute  six-element  open  chain  linkage  in  the  following  manner: 

•  •  •    •  rp 

x  =  H(e)e  +  c(e,e)e  +  G(e)  +  K(e)  k  (40) 

where: 

H(6)  n  x  n  symmetic,  nonsingnlar  moment  of  inertia  matrix 

C(0,6)  n  x  n  matrix  of  centrifugal  and  Coriolis  effects 

G(6)  n  x  1  vector  specifying  the  effects  due  to  gravity 

K(8)     6  x  n  Jacobian  matrix  of  torques  created  at  each 
joint  due  to  external  forces  exerted  on  link  n 

T 
K(O)     transpose  of  K(6) 

k       6x1  vector  of  external  forces  exerted  on  link  n 

x  n  x  1  vector  of  torques  of  each  joint  actuator. 

6       n  x  1  vector  of  joint  variables 
Note  in  the  above  that  the  joint  torques  are  linear  functions  of  the 
joint  accelerations. 

2.   Modelling  Solution  Methodology 

The  methodology  used  to  validate  the  dynamic  model  is  shown 
graphically  in  Figure  6.  Here,  the  torque  was  given  for  a  specific 
instant  in  time.  This  was  then  combined  with  the  present 
experimental  angular  position  and  velocity  as  input  for  equation 
(40).  External  forces  other  than  gravity  where  not  considered  with 
the  input  data  available.  The  angular  acceleration  for  the  same  time 
instant  was  then  computed  from  equation  (40).  A  simple  integration 
was  then  performed  over  the  acceleration  time  domain  to  provide  a 
velocity  and  position  check  with  actual  data.   Figure  7  provides  a 
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Figure   6.      Model   Formulation  Diagram 
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Figure  7.   Computer  Program  Flow  Diagram 
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block  diagram  of  the  steps  (and  associated  subroutines)  which  were 
used  to  implement  this  methodology. 

3.  Simulation  Solution  Methodology 

The  methodology  used  to  simulate  the  dynamic  motion  is  shown 

graphically  in  Figure  8.   As  before,  the  stepped  torque  was  provided, 

with  only  the  initial  velocity  and  position.    The  time  interval 

between  knots  was  divided  into  equally  sized  steps  of  length  At.   The 

acceleration  at  step  i  was  assumed  equal  to  that  at  i+1  and  a 

standard  forward  finite  difference  formulation  was  used  to  determine 

the  estimated  velocity  and  position,  0.,.,  and  6,^,  at  time  t.^^. 

l+l      i+1  l+l 

These  values  were  again  applied  to  equation  (40)  to  recompute  the 

acceleration  at  time  t..„,  for  the  next  time  step.   This  acceleration 

l+l 

was  then  reapplied  to  the  finite  difference  formulas  to  determine  the 
next  positions  and  velocity,  etc.  The  step  size  was  varied  to 
determine  the  effects  on  computer  run  times  and  accuracy. 

4.  Principal  Subroutines 

The  following  descriptions  apply  to  subroutines  found  in  a 

subroutine  called  ROBARM.   ROBARM  was  called  by  a  main  program  as 

often  as  necessary  to  evaluate  the  particular  dynamics  at  a  specific 

knot.   The  recursive  Lagrangian  representation  of  the  dynamic  motion 

equations  [Section  II. F]  was  used  in  the  simulation  method  developed 

by  Walker  and  Orin  [Ref.  3]  to  create  a  FORTRAN  subroutine  named 

.  . . 

ARM1 .   ARM1  allows  input  of  position  6,  velocity  0,  acceleration  0, 

external  forces/moments,  and  joint  forces  and  torques. 
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Figure  8.   Simulation  Formulation  Diagram 
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The  FORTRAN  subroutine,  ARMl(0,0,0,k,T) ,  ultimately  computed 

•   •  « 

H,  the  inertia  term  matrix,  given  6,0,0  and  the  forces  and  moments 

*  « 

exerted  on  link  n,  k.   Another  FORTRAN  subroutine,  ARM2(0,0,r),  is 

•   •  • 

identical  to  ARM1(0,0,0, k,r)  except  that  programming  code  for 
velocity  terms,  gravitational  effects  and  external  forces/moments 
effects,  has  been  eliminated.   Subroutines  ARM1  and  ARM2  form  the 
basic  programs   used  for   simulating   the   rigid  body  manipulator 
dynamics.   An  explicit  analytic  expression  for  each  of  the  terms  of 
motion  equations  is  not  required  for  the  simulation. 
5.   Determination  of  Acceleration 

A  bias  vector,  b,  is  set  equal  to  the  torques  due  to  gravity, 
centrifugal  and  Coriolis  accelerations,  and  external  forces  and 
moments  on  link  n.   Therefore, 

b  =  C(0,0)0  +  G(0)  +  K(0)T  k  (41) 

Thus,  the  joint  variable  accelerations  can  be  obtained  by  solving  a 
linear  matrix  equation 

H(0)0  =  (t  -  b).  (42) 

The  bias  vector  b  is  computed  by  setting  O,  O  and  k  to  their  current 
state,  and  letting  0=0.   ARM1  is  then  called  in  the  following 
manner:  ARM1 (0,0, 0,k,b) .   Given  these  inputs,  the  bias  vector  b  is 
computed  in  ARM1. 

The  difficult  part  in  solving  the  above  linear  equation  is  in 
the  matrix  H  element  evaluations.   This  is  done  by  setting  O  to  its 
current  state.   ARM2  is  then  called  in  the  following  manner: 
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ARM2(6, e . ,h  ) .   Here,  e.  is  a  n  x  1  vector  with  the  jth  element  equal 

J     J  J 

to  1,  all  other  values  of  e  are  equal  to  0.   The  variable  h.  is  the 

J 

jth  column  of  H  and  represents  the  joint  actuator  torque  when  the 
joint  velocities  are  zero.   When  the  H  matrix  elements  aredetermined, 
the  joint  accelerations  can  be  obtained  by  solvingequation  (42). 
6.   Program  Flow 

Summarizing,  the  computational  procedures  are: 
MAIN  PROGRAM  (process  trajectories  and  torques) 
SUBROUTINE  CONST 
SUBROUTINE  TRAJ 

•   •  • 

SUBROUTINE  ROBARM  (Knot  #,e,e,6,T,k,PLYLD,  t) 
SUBROUTINE  KINEMT  (6,PLYLD) 
SUBROUTINE  ARM1  (e,O,0,k,B) 
i  =  1  —  >  6 

SUBROUTINE  ARM2  (6,e,H) 
I  =  t  -  B 
SUBROUTINE  TDDSOL  (H,6,X,  error  status) 
SUBROUTINE  VELSOL  (t,  6, 6,  Knot  #) 
SUBROUTINE  POSSOL  ( t, 6, 6, 6,  Knot  #) 

•   •  • 

SUBROUTINE  ROBSIM  (Knot  #,6,e,e,T,k,PLYLD,  t) 

where  subroutine: 

CONST  -  Loads  constants,  including:  radii  of  gyration,  masses,  arm 
offsets,  differential  matrices,  gravity  constants. 

TRAJ  -  Loads  trajectory  data,  either  from  point-to-point  information 
or  a  fitted  curve.   Includes  time,  torque,  velocity,  and  position. 


54 


ROBARM  -  Central  umbrella  subroutine,  called  by  main  program  as  each 
trajectory  point  is  processed.   Variable  PLYLD  allows  program  to 
adjust  for  playload  weights  and  variable  t  is  a  time  interval  (sec). 

KINEMT  -  Calculates  the  link  and  transformation  matrices. 

ARM1  -  Subroutine  which  normally  calculates  link  torques,  given  link 
position,  velocity,  acceleration  and  external  forces,  however  in  this 
simulation  method,  was  used  to  calculate  the  inertia  matrix  terms. 

ARM2  -  Same  as  ARM1,  with  Coriolis,  gravity,  and  coupling  terms 
removed. 

TDDSOL  -  Linear  simultaneous  equation  solver  using  trianglur 
decomposition  method. 

VELCTY  -  Integration  subroutine  used  to  solve  for  velocity  given 
acceleration,  using  the  trapezoidal  method. 

POSSOL  -  Integration  subroutine  to  solve  for  position  using  the 
Hermitian  form. 

ROBSIM  -  Finite  difference  simulation  subroutine. 

7.  Program  Algorithm  .Development 

The  maximum  number  of  links  allowed  in  this  program  is 

m 

fifteen  (15).  Thus,  matrix  maximum  dimensions  are  configured  by  row, 
column,  and  link  number.  For  example,  the  dimensioning  of  the  link 
coordinate  matrix  is  A(4,4,15).  There  are  no  serious  limitations 
with  these  dimensions  when  using  a  large  capacity  computer  (i.e.  IBM 
3033  system) . 

The  maximum  number  of  knots  which  can  evaluated  is 
arbitrarily  set  at  one  hundred  (100).  This  number  can  be  changed 
significantly,  without  seriously  overloading  memory  allocations. 

The  specific  numerical  values  and  equations  used  to  develop 
the  computer  program  are  provided  below. 
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a.   Subroutine  CONST 

(1)  DATA  Input.   The  relative  link  masses  (kg)  for  PUMA 
ARM  600  are: 


33.5 
77.3 
36.3 
8.95 
2.39 
1.00  J 


the  radii  of  gyration  (cm2)  are: 


k2  = 
xx 


the  first  moments  (cm)  are: 


0.0 
-21.6 
0.0 
0.0 
0.0 
0.0 


451.0  - 

'  451.0  " 

'   57.9 

565.7 

1847.0 

1408.0 

672.8 
31.6 

k2  = 

yy 

679.1 
21.1 

k2  = 
zz 

36.0 
31.6 

6.9 

11.2 

6.9 

33.8  . 

.  33.8  . 

L    .911 

X   = 


y  - 


0.0  " 

8.0  " 

0.0 

21.75 

0.0 
2.0 

z  = 

21.6 
0.0 

0.0 

0.0 

0.0  . 

.  1.0  . 

and  the  link  parameters  for  the  revolute  arm  are: 


-90.0 

0.0 

90.0 

-90.0 

90.0 

0.0 


6  = 


r e*  i 

*z 

% 

% 

e, 

L  e6  J 

d  = 


0.0 
0.0 
12.5 
43.2 
0.0 
0.0 


a  = 


0.0 
43.2 
1.9 
0.0 
0.0 
0.0 
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(2)  Calculations.   Recalling  equation  (19),  the  above 
data  is  used  to  compute  the  Inertia  matricies: 


J.  =  m 


2       2       2 

k.   +k.   -k. 

jyy  jzz  jxz   ka 


2 

k. 


2 

k. 
jxz 


2       2       2 

k.   -k.   +k. 

j"  jyy  jzz 


J" 


jyz 


jyz 


2       2       2 

k,   +k.   -k. 
jxx  jyy   jzz 


z . 
J 


X. 

J 


(19) 


(3)  Programming  Philosophy*   The  physical  and  geometric 

data  is  loaded  from  a  disk  file  by  the  CONST  subroutine.    The 

calculation  in  this  subroutine  is  only  called  once,  as  values  therein 

remain  unchanged  during  all  conditions.    The  data  includes:   the 

number    of    links;    the    type    of    link    (prismatic=[l]    or 

translational-[2] ) ;  the  Denavit-Hartenberg  a,  d,  a,  6  parameters,  the 

radii  of  gyration;  the  centers  of  mass;  and  the  link  masses.   The  0 

(zero)  matrix  is  then  loaded  and  parameter  a  is  converted  to  radians. 

The  0  matrix  is  used  in  subroutine  ROBARM. 

In  addition  to  computing  and  filling  the  J.  matrix 

and  r  vectors,  the  A  and  T  matrices  are  initialized  to  zero.   The  A 

o 

and  T  matrices  are  filled  as  identity  matrices. 
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b.   Subroutine  KINEMT 

(1)  Calculations.   The  link  coordinate,  first  and  second 
partial  matrices  are  calculated  using  equation  (1),  repeated  below: 


aj  s 


dA, 


30. 


a*A. 
J 


ae.» 
j 


cos  0. 


sin  6. 
J 


■sin  0. 
J 

cos  0. 


-cos  e 


-sin  0 
0 
0 


-sin  6.  cos  o.    sin  6.  sin  a.     a.  cos  0. 


cos  0.  cos  a.   I  -cos  0.  sin  a.    a.  sin  0 


j 
sin  a 


J 


J 
cos  a 


J  i   J 


j 


(1) 


-cos  0.  cos  a.    cos  0.  sin  a.   I  -a.  sin  0, 


-sin  0.  cos  a.  I  sin  0.  sin  a.     a.  cos  0 


j 


j 


I 


(43) 


sin  0.  cos  a.  I  -sin  0.  sin  a.  I  -a.  cos  0. 

j      J  ,       J      j  |  J  J 

-cos  0,  cos  o.    cos  0.  sin  a.  I  -a.  sin  0. 

j      j  ,      J      j  ,  J  J 


I 


0 
0 


(44) 


Using  the  above,  T.  is  calculated  using  equation  (2) 

•I 


and  equation  (33) : 


•.  *  T*  * 


J 

n  a 

i=i 


T.  ,  A. 
J-l  J 


(2)  Programming  Philosophy*  The  subroutine  is  called  by 
ROB ARM  at  each  knot.  The  calculations  preformed  in  this  subroutine 
are  dependent  on  the  value  of  link  'variables'  and  change  at  each 
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knot  calling.  In  the  cases  discussed  in  this  thesis,  all  the 
revolute  ''variables''  are  9.  The  constant  physical  and  geometric 
data  are  passed  from  the  CONST  to  the  KINEMT  subroutines  via  a  COMMON 
statement  (ROB2,ROB3).  These  parameters  are  used  to  compute  all  of 
the  A,  3A/36  and  32A/362  matrices  for  links  1  to  6.  As  previously 
stated,  these  and  other  4x4  matrices  are  configured  in  the  program  as 
by  [row,  column,  link  number]. 

Upon  determination  of  the  A  matrices,  the  values  of 
TL ,  T„ ,  T, ,  T,,  T_,  and  T,  are  computed  using  Equation  (2). 

1    l  5  4    5         0 

Finally,  the  gravity  vector  is  loaded  so  that  gravity 

acts  in  the  positive  z   direction.   All  these  vectors  and  the  above 

o 

matrices   are   passed   to   other  programs  via   a   COMMON   statement 

(ROB8,ROB9). 

c.   Subroutine  ARM1 

(1)  Calculations.   Using  equation  (34)  and  equation 

(35),  T. ,  T.  and  T.  are  computed  from  j=l  to  j=6: 
J    J      J 

•     .  dA.  . 

T.  =  T.  ,  A.  +  T.  ,  — ^  9.  (45) 

J    J-l  J    J-l  ae   J 

J 

....  •    dA,  .         d3A.  .         dA.  .. 

T.  =  T.  i  A,  +  2  T.  ,  — ^  6,  +  T.  .  ^  0?  +  T.  .  — i  0.    (46) 

J     J-l   J      J-l  ae   J     j-l  ae  a   J     J-l  ae   J 

J  J  J 


Additionally,       3T.  dk,       . 

_1  =  T  _i  t! 

ae,   J  ae.  J 

j  j 


(47) 
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When  T. ,  T. ,  and  T.  are  computed,  D.  and  c.  is 
J    J       J  11 

computed  from  j=6  to  j=l  using  equation  (37)  and  equation  (38) : 


J.  T!  +  A...  D.  ...      , 
J   J     J+1   J+l    and 


c.   =   m.   r.  +  A.  .,  c.  ., 
J       J    J     J+l   J+l 


re: 

■  o  ■ 

Xl" 

•  0  " 

1 
rl   = 

0 
.   1    . 
■  0  ' 

2 
r2  = 

0 

L  l  J 
"  °  1 

3 
r3  = 

0 

Z3 

L  l  J 
•  0  " 

4 
r4  = 

0 

.  1  . 

5 
r5  = 

0 

Z5 

L  l  J 

6 
r6  = 

0 

z« 

L  l  J 

The  torques  in  a  revolute  manipulator  arm  are  then  computed  by 
rewriting  equation  (39) : 


v.    =   tr 


3T. 


ae. 


8T 
T   j 
g  — ^  c. 


(48) 


ae 


j 


(2)  Matrix  Multiplication  and  Addition  Subroutines.   The 
following  subroutines  are  called  during  ARM1  and  ARM2  to  perform 
repeated  multiplication  and  additions  on  indexed  4x4  and  4x1 
matrices: 

MAT44  -  Multiples  a  4x4  matrix  and  a  4x4  matrix 

MAT44T  -  Multiples  a  4x4  matrix  and  a  4x4  Transposed  matrix 

MATC4  -  Multiples  a  Constant  and  a  4x4  matrix 

MATC1  -  Multiples  a  Constant  and  a  4x1  matrix 

MAT41  -  Multiples  a  4x4  matrix  and  a  4x1  matrix 

ADD44  -  Adds  a  4x4  matrix  to  another  4x4  matrix 

ADDALL  -  Adds  4  different  4x4  matrices 

ADD11  -  Adds  a  4x1  matrix  and  a  4x1  matrix 

FIRST  -  Finds  the  trace  of  two  multiplied  4x4  matrices  to  find 
the  left  (first)  side  of  the  force  equation  (48). 

SECOND  -  Multiplies  a  1x4  matrix  and  a  4x4  matrix,  and 

subsequently  another  4x1  matrix  to  find  the  right 
(second)  side  of  the  force  equation  (48). 
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(3)  Programming  Philosophy.  ARM1  and  ARM2  contained  many 
short  subroutine  calls  which  are  used  to  multiply  and  added  matrices. 
A  variety  of  real  constant  arrays  are  declared  for  use  in  the  ARM1 
and  ARM2  programs  (CI  to  C15) .  These  area  provided  to  ''hold'* 
preliminary  calculated  data  for  further  use  by  other  subroutines. 
The  use  of  many  rather  than  few  holding  matrices  is  based  on  the  lack 
of  memory  constraints,  and  the  avoidance  of  the  need  to  constantly 
reinitialize.  The  use  of  these  holding  matrices  can  be  avoided  by 
rewriting  the  matrix  multipling  and  adding  subroutines  so  there  is  no 
requirement  for  holding. 

The  first  terms  calculated  are  T,  from  j=l  to  6. 
When  j=l,  the  left  side  of  equation  (48)  vanishes  leaving  only 
the  right  side.   This  condition  is  satisfied  using  an  ''IF''  loop. 

Similiar  equation  ''reduction*'  conditions  exist  for 

the  T,  D. ,  C,  and  3T./96.  matrices.   The  D.  and  C,  matrices  are 
i    i        J    J  i      i 

filled  in  reversed  order  (backward  recursion) . 
d.   Subroutine  ARM2 

(1)  Calculations.   Rewriting  equation  (35),  T  is 


computed  from  j=l  to  j=6: 


3A.  .. 


Tj  =  Vl  Aj  ♦  Vl  -J.  9j  (49, 

*   • 

When  T. ,  T,  are  computed,  D.  is  computed  from  j=6  to  j=l  using 
equation  (38) : 

D.   =   J,  T!  +  A.A1  D.  .i 
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The  torques  are  finally  computed  using: 

9T. 


T.  =   tr 
J 


ae.  J 
j 


(50) 


(2)  Programming  Philosophy.   The  memory  requirements  for 

ARM2  are  less,  since  the  velocity  and  gravity  terms  are  removed. 

Furthermore,  there  is  no  requirement  for  calculation  of  T,  since 

T  is  0,  and  all  velocity  terms  are  removed,  leaving  all  subsequent 
o 

terms  to  equal  0. 

Additionally,  the  C.  terms  are  not  required  as  they 

are  used  with  gravity  terms  in  the  force  equation.    The  3T./36. 

J    J 

matrices  are  calculated  in  ARM1  and  their  values  remain  unchanged 
from  those  used  in  ARM1. 

e.   Subroutine  TDDSOL 

The  subroutine  TDDSOL  is  an  IMSL  simultaneous  equation 
solver  called  LEQIF.  This  subroutine  provides  a  solution  to  linear 
equation  generated  by  the  inertia  and  bias  matrices,  using  full 
matrices  in  virtual  memory.  The  program  listing  is  not  provided  for 
this  copyrighted  material  (1982,  by  IMSL,  INC.).  The  subroutine 
calls  other  IMSL  subroutines.  However,  any  linear  equation  solving 
subroutine,  with  capabilities  to  warn  of  matrix  singularities  can  be 
used.         Argument     input     for     this     subroutine     was: 

Argument  #  1  -  Input  n  x  n  'A'-matrix  of  equation  AX=B 

2  -  Rows  in  'A'  as  dimensioned  by  calling  program 

3  -  The  order  of  matrix  'A' 

4  -  Equal  to  n  (related  to  program  speed) 

5  -  Right  hand  side  of  equation  AX  =  B. 

at  output,  solution  matrix  X  replaces  B. 

6  —  Rows  in  'B'  as  dimensioned  in  calling  program 
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7  -  Number  of  columns  in  'B' 

8  -  1=0,  Code  to  factor  matrix  and  solve  equation  AX=B 

9  -  Real  work  area  =  3*n 

10  -  Error  parameter  IER=129  — >  Matrix  a  singular  AX=B 

f.  Subroutine  VELSOL 

Subroutine  VELSOL  was  written  as  a  simple  adaption  of 

trapezoidal  rule  [Ref.  47  j  page  75],   The  subroutine  provided  the 

velocity  solution  by  integrating  angular  acceleration  over  the  entire 

time  considered.   The  argument  input  for  this  subroutine  was: 
Argument  #  1  -  Time,  all  time  point  covered  to  point  of  calling 

2  -  Acceleration,  submitted  one  link  a  time 

3  -  Velocity,  output 

4  —  Link  number 

g.  Subroutine  POSSOL 

Subroutine  POSSOL  is  written  using  an  adaption  of  a 
Hermite  interpolation  method  of  integration  [Ref.  47;  page  314]. 
This  method  provides  a  more  accurate  solution  as  derivatives  are 
known.   The  argument  input  for  this  subroutine  is: 

Argument  #  1  -  Time,  all  time  point  covered  to  point  of  calling 

2  —  Acceleration,  submitted  one  link  a  time 

3  -  Velocity,  from  previous  subroutine 

4  -  Position,  output 

5  —  Link  number 

h.   Subroutine  ROBSIM 

This  subroutine  implements  the  simulation  technique 
described  in  section  III.C.3.  The  simulation  program  is  called  by 
setting  ISIM  equal  to  one  (ISIM=1)  in  the  main  program  (ROBOT).  When 
this  subroutine  is  called,  all  output  can  be  sent,  via  a  separate 
subroutine,  to  the  same  position,  velocity,  and  acceleration  files  as 
the  modelling  program.    These  files  are  for  use  with  graphics 
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programs.  This  subroutine  computes  new  position  and  velocity 
information,  and  calls  ARM1  and  TDDSOL  to  determine  the  new 
acceleration  at  each  step.  Step  sizes  are  determined  by  setting  the 
number  of  steps  in  the  main  program  (ISTEP  =  number  of  steps). 
Torque  is  keep  constant  as  the  subroutine  is  called  at  each  knot. 
Arguments  passed  to  this  subroutine  are  identical  to  those  passed  to 
ROBARM.  Only  the  knot  number,  torque,  and  the  time  matrix  are  used 
throughout  an  entire  program  run.  Arguments  for  position  and 
velocity  are  used  only  on  the  first  knot. 

D.   DATA  INPUT 

Discrete  torque  data  is  fed  into  the  modelling  and  simulation 
process  at  each  trajectory  knot.  An  analysis  of  the  relative  joint 
moment  magnitudes  (except  friction)  was  performed  for  the  PUMA 
manipulator  by  R.P.  Paul  [Ref.  46].  Paul  estimated  the  values  for 
the  link  masses  and  radii  of  gyration  based  on  a  manipulator 
examination.  The  actual  mass  and  radii  of  gyration  values  were 
obtained  analytically,  and  were  within  an  estimated  10%  of  actual 
values.  Paul's  results  were  used  as  input  data  for  this  simulation. 
This  input  data  includes  all  robot  link  inertias,  motor  torque 
characteristics,  and  accuracies.  The  torque  inputs  and 
velocity/positional  output  data  was  obtained  through  use  of  feedback 
controller  sensors,  and  its  accuracy  is  estimated  with  1%  to  2%. 


64 


E.   PROGRAM  OUTPUT 

Simulation  program  output  included  the  position,  velocity,  and 

acceleration  versus  time  data  for  the  end  effector  and  each  joint. 

This  data  was  placed  in  a  data  file  which  easily  interfaced  with 

a  graphics  interpreter  program.   Data  output  included: 

Desired  Position  vs  Computed  Position  files  (PEn  DATA) 
Desired  Velocity  vs  Computed  Position  files  (VEn  DATA) 
Desired  Acceleration  vs  Computed  Acceleration  files  (AEn  DATA) 
Error  evaluation  files  (ERROR  DATA) 

Additionally,  an  independent  subroutine  (TRANSP)  was  developed  to 

calculate  the  end-effector  transposition  matrix  using  the  actual  and 

computed  position  files.   The  transposition  matrix  was  then  used  as 

output  for  this  subroutine,  providing  the  x,y,z  coordinates  of  the 

end-effector.   Data  output  included: 

Actual  path  x,y,z  coordinates  (PATHACT  DATA) 
Modelled  path  x,y,z  coordinates  (PATHMOD  DATA) 
Simulation  path  x,y,z  coordinates  (PATHSIM  DATA) 

This  data  was  used  to  provide  a  graphical  representation  of  the 

end-effector  path. 
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IV.   RESULTS  AND  DISCUSSION 

A.   MODELLING  PROGRAM  RESULTS 

Based  on  the  dynamic  model  formulation  presented  in  this  thesis, 
a  general  computer  program  package  for  the  dynamic  analysis  of  a 
revolute  six  joint  manipulators  was  developed. 

The  input  torque  for  each  joint  was  supplied  as  a  series  of 
trajectory  knots,  as  shown  in  Figure  9  for  a  typical  joint.  The  only 
arm  external  loading  terms  considered  were  those  of  gravity.  Angular 
position,  velocity,  and  acceleration,  were  subsequently  computed  and 
plotted  against  actual  knot  values  to  verify  the  accuracy  of  the 
model,  as  shown  typically  for  joint  2  in  Figures  10,  11,  and  12  (A 
complete  set  of  all  graphs  for  torque,  acceleration,  velocity,  and 
position   are   available   in   Appendix   8,   Figures   19-42.) 

Table  I  provides  a  numerical  average  modelling  error  for  each 
link  for  the  trajectory  terms.  This  error  was  obtained  *  by 
determining  the  difference  between  the  actual  data  and  the  model 
results,  and  then  normalizing  with  the  actual  data.  The  errors  were 
summed  over  all  knots  and  averaged. 

Table  I.   Modelling  Joint  Position  Errors 

LINK  NUMBER  12       3       4       5       6 


Acceleration 

Max  Error  (  /sec*) 

Velocity 

Max  Error  (  /sec) 

Position 

o. 
Max  Error  (  ) 


2.778%  1.492%     2.201%     1.244%     2.543%  2.510% 

0.152  0.494        1.542        0.103        1.222  0.552 

6.457%  4.404%     3.091%     2.824%     5.397%  4.283% 

2.306  1.267        2.050        1.487        3.055  1.687 

7.210%  8.156%     5.979%     9.640%  13.386%  5.763% 

10.871  8.154     13.926     12.968     13.899  8.170 
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After  all  trajectory  knots  had  been  analyzed  by  the  main  program, 
the  predicted  joint  model  positions  were  supplied  to  another 
independent  computer  program  TRANSP  (similiar  to  KINEMT) ,  where  the 
angular  positions  of  each  link  were  transposed,  from  base  to  end- 
effector,  to  determine  the  position  vector  of  the  end-effector.  The 
motion  produced  in  each  plane,  XY,  XZ,  and  YZ,  was  then  plotted 
graphically,  as  seen  in  Figures  13—15.  Table  II.  provides  the 
average  error  achieved  while  the  arm  moved  over  the  specified  path. 
Again,  this  error  was  obtained  by  determining  the  difference  between 
the  actual  data  and  the  model  results,  normalizing  with  the  actual 
data,  and  again  averaging  over  the  range  of  knots.  The  errors 
translate  to  a  maximum  path  variation  of  approximately  12  cm. 

Table  II.  End-effector  Modelling  Errors 


Knots  # 

X  coord 

Y  coord 

Z  coord 

Avg  Error 

22.72% 

12.25% 

11.08% 

Max  Error 

27.4cm 

4.1cm 

7.1cm 

B.   SIMULATION  PROGRAM  RESULTS 

The  use  of  the  simulation  subroutine  was  bypassed  (ISIM=0)  during 
the  above  operations.  This  was  done  to  allow  modelling  without  the 
the  simulator  program.  When  the  simulation  subroutine  was 
incorporated  into  the  program  (ISIM=1),  several  simulations,  using 
varying  stepsizes,  were  evaluated  for  the  series  of  knot  points. 
Torques  were  held  constant  during  simulations  between  knot  points. 
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In  addition  to  noting  errors  achieved  during  each  evaluation,  the 
time  required  to  run  the  simulator  program  on  the  IBM  3033  was  also 
recorded.   This  information  is  provided  in  Table  III  and  Figure  16. 

Table  III.  CPU  Time  Required  to  Process  Simulation  Program  (Seconds) 


Knots 

Number  of 

Steps  Pro 

cessed  between  Knot 

s 

Processed 

5 

10 

15 

20 

25 

30 

3.0 

1.53 

3.03 

3.43 

6.01 

6.37 

9.06 

5.0 

1.84 

4.08 

6.65 

9.61 

12.10 

17.30 

10.0 

3.34 

8.22 

14.10 

20.20 

26.40 

37.40 

15.0 

4.57 

12.20 

20.20 

30.60 

40.50 

60.30 

20.0 

5.74 

17.50 

26.80 

38.30 

50.80 

79.10 

25.0 

7.17 

20.20 

33.00 

48.00 

63.00 

99.20 

30.0 

8.43 

24.00 

39.40 

57.30 

75.20 

125.20 

35.0 

9.69 

28.50 

45.40 

68.50 

88.00 

138.20 

37.0 

10.40 

29.80 

48.30 

71.50 

94.30 

152.30 

Once  again,  acceleration,  velocity,  and  position  outputs  were 
obtained  and  the  results  were  recorded  graphically,  as  shown  in 
Appendix  B.  The  trajectory  information  for  joint  1  is  provided  in 
Figures  17-19,  other  results  are  included  in  Appendix  B. 

C.   DISCUSSION 
1.   Model 

The  most  important  result  achieved  was  that  the  program 
demonstrated  the  capabilities  to  generate  a  forward  dynamic  solution 
and  serve  as  an  accurate  dynamic  robot  model.  Linear  programming 
techniques  were  used  to  determine  the  angular  acceleration.  This 
program  was  successfully  tested  against  known  results  for  a  given 
trajectory  of  the  PUMA  arm. 
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The  acceleration  data  supplied  was  determined  analytically^ 
however,  the  method  used  to  determine  this  data  was  not  available. 
The  small  acceleration  errors  achieved  gives  rise  to  a  suspicion  that 
the  analytical  process  may  also  have  included  joint  torque 
information,  possibly  processed  through  a  dynamic  formulation  program 
similar  to  that  presented  in  this  thesis. 

The  accuracy  of  the  data  which  describes  a  robot's  joint 
links,  mass,  to  a  set  of  general  inverse  kinematic  equations  was 
estimated  at  ±10%  [Ref.  46],  The  physical  property  data  was 
originally  obtained  by  ''fitting'*  the  manipulator  arm's  mass  and 
inertia  terms  to  a  point  where  they  are  consistent  with  the  robot's 
motions.  The  error  achieved  by  the  model  appears  to  be  well  within 
results  which  could  be  expected  from  the  data  provided. 

It  was  noted  that  the  model  end— effector  path  tracked 
erratically  in  all  knots  after  15  seconds.  The  source  of  these 
errors  can  be  seen  by  inspecting  the  angular  position  verses  time 
figures  for  each  joint,  and  observing  that  the  individual  joint 
errors  increase  at  times  after  IS  seconds.  Although  at  worst  these 
errors  are  relatively  small  (7-8%),  they  accumulative  towards  the 
end-effector.  This  is  due  to  the  nature  of  process  involved  in 
transposing  end-effector  coordinates  to  base  coordinates. 
Consequently,  this  produces  a  12-13%  error  in  end-effector  position. 
This  error  seems  reasonable,  considering  the  low  magnitude  of 
individual  joint  errors. 
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The   thesis   display   program   (TRANSPLT)   was   capable   of 
displaying  motion  data  in  a  two  dimensional  plane  and  provided  an 
acceptable,  but  limited  method  of  visualizing  robot  motion  in  order 
to  evaluate  robot  performance. 
2.   Simulation 

The  accuracy  of  the  6  link  simulation  program  deteriorated 
rapidly  after  five  seconds  of  simulated  performance.  At  that  time 
the  accelerations  oscillated  rapidly  about  the  actual  values,  with 
magnitudes  greater  than  500%.  Velocity  and  positional  results  were, 
consequently,  several  orders  of  magnitude  different  than  expected 
results.  In  an  effort  to  eliminate  the  possibility  that  excessive 
errors  in  kinematic  variables  near  the  end-effector  were  creating 
large  errors  in  the  first  three  links,  the  masses  of  links  4,  5,  and 
6  were  combined  arithmetically,  and  the  program  was  rerun  as  a  four 
link  problem.  While  acceleration  results  did  not  change  under  this 
second  evaluation  trial,  velocity  and  positional  data  were  generally 
as  expected  for  the  first  5  seconds.  Typical  velocity  results  are 
shown  in  Figure  18.  The  remainder  of  the  disscussion  will  address 
the  second  (four  link)  evaluation. 

A  detailed  examination  was  made  to  determine  the  nature  of 
the  oscillations  for  a  segment  between  two  knots.  At  the  beginning 
of  each  segment,  a  new  torque  value  is  supplied  to  the  simulator. 
This  torque  value  does  not  change  further  until  the  next  segment  is 
evaluated.  While  velocities  and  positions  form  an  integral  relation 
with  accelerations,  they  are  not  treated  as  such  in  the  non— iterative 
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constant  acceleration  approximation  scheme  implemented  in  the  thesis 
simulation  routine.  Consequently,  as  each  succeeding  step  is 
evaluated,  the  method  produces  velocities  and  positions  which  become 
increasingly  more  inaccurate  as  the  time  evaluation  proceeds.  Since 
these  values  are  supplied  to  the  matrix  equation  for  the 
acceleration,  it  can  be  seen  that  the  acceleration  errors  will 
increase  as  velocity  and  position  errors  compound. 

Values  in  the  inertia  matrix,  which  are  supplied  as  the  left 
side  of  the  matrix  dynamic  equation,  were  examined  at  all  evaluation 
points  in  the  belief  that  the  deviations  in  acceleration  may  have 
resulted  from  singularities  produced  in  a  sparsely  occuppied  matrix. 
However,  singularity  conditions  were  not  observed. 
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A.  MODEL 

A  computer  program  was  written  which  allowed  the  calculation  of 
open  spatial  mechanical  manipulator  chain  dynamics  based  on  the 
application  of  independent  equations  represented  in  Hollerbach's 
recursive  Lagrangian  form.  The  resulting  dynamic  formulation  was 
developed  in  numerical  matrix  form  rather  than  as  specific  equations 
of  motion.   The  model  was  evaluated  for  the  PUMA  robot  manipulator. 

In  view  of  the  inaccuracies  in  model  constants  it  is  difficult  to 
evaluate  the  accuracy  of  calculated  variables.  However,  the  model 
did  produce  results  of  sufficient  accuracy  that  it  could  provide 
insight  into  the  dynamic  properties  of  the  manipulator  arm,  and  could 
give  assistance  in  the  task  of  trajectory  planning. 

B.  SIMULATION 

The  simulation  routine  did  not  produce  reliable  or  accurate 
results  due  to  the  programs'  inability  to  continuously  provide  values 
of  velocity  and  position  which  would  satisfy  their  integral  relation 
with  acceleration.  The  linearizing  approximation  of  stepwise 
constant  acceleration  was  a  poor  approach. 
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C.      FUTURE  WORK 

1.  Program  Enhancements 

The  simulation  routine  must  be  rewritten  to  iterate  about 
each  acceleration  step  until  accurate  values  of  velocity  and  position 
are  obtained  for  the  next  step. 

An  entire  robot  arm  system  can  be  modelled  by:  (1)  modelling 
of  an  operational  amplifier  which  can  convert  the  position  error 
signal  into  a  current  to  drive  a  servo  valve,  (2)  modelling  a 
hydraulic  actuator  system  (which  can  be  a  second  order  model  for  a 
flow  control  valve,  and  an  integrator  to  model  the  actuator),  and  (3) 
connecting  the  output  of  the  hydraulics  system  (torque)  to  the  input 
for  the  thesis  model  in  order  to  predict  joint  accelerations, 
velocity,  and  position. 

The  robot  modelling  could  be  modified  to  accommodate  other 
manipulators  with  Euler  or  cylindrical  coordinates.  Additionally, 
although  the  discussion  in  this  thesis  deals  primarily  with  open 
chain  systems,  the  program  can  be  readily  extended  to  treat  closed 
loop  systems. 

2.  Computers 

Future  enchancements  of  the  thesis  program  could  include 
better  user  interfacing  (''user  friendly*'),  more  efficient  code, 
better   utilization   of   memory,    and   improved   input/ output. 
Additionally,   three   dimensional   kinematics   representations   be 
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designed  by  interfacing  the  program  with  CAD/CAM  or  finite  element 
software. 

Microcomputer  (Personal  Computer)  solutions  to  applications 
problems  will  become  a  common  practice  among  manufacturing  engineers 
in  the  coming  years.  It  is  recommended  that  the  FORTRAN  program  be 
compiled  and  run  on  a  microcomputer,  which  could  ultimately  can  be 
used  to  control  an  actual  manipulator  arm.  Memory  allocation  might 
become  a  serious  problem  if  this  thesis  program  is  transferred  to  a 
smaller  micro-  or  mini-computer  system.  A  benefit  of  using  a 
dedicated  mini-  or  microcomputer  would  be  the  decreased  dependence  on 
time-shared  mainframe  computer  time. 

Additionally,  the  program  can  be  modified  for  double 
precision  to  increase  accuracy,  particularly  around  suspected 
singularity  points,  at  an  expense  of  increased  total  computation 
time. 

3.  Physical.  Analysis 

The  physical  and  geometric  parameters  of  robot  arm  components 
are  perhaps  the  most  difficult  to  obtain.  In  addition  to  published 
data,  generalized  methods  are  available  which  can  be  used  to  obtain 
these  values.  The  program  can  be  used  to  analyze  the  physical 
parameters  of  small  laboratory  manipulator  arms  by  varying  dynamic 
parameters  estimates  to  match  known  motion  data.  Such  a  model  can  be 
coupled  directly  to  a  robot  arm  control  programs,  where  results  for 
each  of  the  major  arm  linkages  and  control  components  can  be 
validated  experimentally. 
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The  modelling  program  can  be  used  to  evaluate  the  relative 
effects  of  acceleration  and  velocity  on  various  terms  in  the  dynamic 
equations. 

4.  Control.  Design 

In  its  present  form,  the  program  permits  development  of  robot 
and  workspace  models,  robot  motion  programming,  and  duty  cycle 
calculations,  such  as  forces,  torques,  and  deflections. 

When  using  the  program  to  design  workcells  by  producing  two- 
dimensional  diagrams  of  manipulator  motion,  work  points  can  be 
defined,  and  a  motion  path  can  be  analyzed  to  allow  collision 
avoidance  evaluations. 

The  simulation  can  be  used  to  determine  the  effects  of 
variables  on  dynamic  performance,  to  develop  robot  dynamic  analysis 
data,  and  to  identify  and  evaluate  robot  singularity  conditions. 
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APPENDIX  A 

RULES  GOVERNING  LINKS,  JOINTS,  AND  THEIR  PARAMETERS 

1.  Mechanical  manipulators  consist  of  a  sequence  of  rigid  bodies, 
called  links,  connected  by  either  revolute  or  prismatic  joints.   Each 
joint-link  pair  constitutes  one  degree  of  freedom. 

2.  An  n  degree-of-f reedom  manipulator  has  n  joint-link  pairs  with 
link  0.   Link  0  is  not  considered  part  of  the  robot 

3.  Link  0  is  attached  to  a  supporting  base  where  an  inertial 
coordinate  frame  is  established.   Last  link  can  have  a  tool. 

4.  Joints  and  links  are  numbered  outward  from  the  base 

5.  Each  link  is  connected  to  two  others  at  most  so  that  no  closed 
loops  are  formed. 

6.  A  joint  axis  (for  joint  i)  is  established  at  the  connection  of 
two  links.   This  joint  axis  has  two  normals  connected  to  it,  one  for 
each  link. 

7.  The  relative  position  of  two  such  connected  links  (link  i-1  and 
link  i)  is  given  by  d.,  the  distance  measured  along  the  joint  axis 
between  normals. 

8.  The  joint  angle,  9,,  between  the  normals  is  measured  in  a  plane 
normal  to  the  joint  axis.   They  determine  the  relative  position  of 
neighboring  links. 
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9.  A  link  i  is  connected  to  two  other  links. 

Two  joint  axes  are  established  at  both  ends  of  connection. 

Links  maintain  fixed  configuration  between  their  joints. 

Configuration  structure  is  characterized  by  a.  and  a. 

1      1 

d.  and  6.  determine  the  neighbor  link  relative  positions 

10.  There  are  a.,  a.,  d.  and  6.  parameters  associated  with  each 

i   i.   i      i 

manipulator  link  and  they  represent  the  minimal  sufficient  set  to 
determine  complete  kinematic  configuration  of  each  robot  arm  link, 
a.  =  length,  shortest  distance  along  common  normal  between  joint  axes 
a.  -  twist*  angle  between  joint  axes  in  plane  perpendicular  to  a., 
d.  =  distance  between  adjacent  links. 
6.  -  angle  between  adjacent  links. 


DENAVIT-HARTENBERG  REPRESENTATION 
The  Denavit-Hartenberg  representation  is  a  matrix  method  of 
systematically  establishing  a  coordinate  system  (body-attached  frame) 
for  each  link  of  an  articulated  chain.   A  4x4  homogeneous 
transformation  matrix  represents  each  link's  coordinate  system  at  the 
joint  with  respect  to  the  previous  link's  coordinate  system. 

An  orthonormal  Cartesian  coordinate  system  is  established  for 
each  link  at  its  joint  axis,  plus  the  base  coordinate  frame.   Since  a 
rotary  joint  has  only  one  degree  of  freedom,  each  robot  arm 
coordinate  frame  corresponds  to  joint  (i+1)  and  is  fixed  in  link  i. 
When  an  actuator  activates  joint  i,  link  i  moves  with  respect  to  link 
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(i-1).   Since  i's  coordinate  system  is  fixed  in  link  i,  it  moves  with 

link  i. 

The  nth  coordinate  frame  moves  with  the  tool  or  hand,  (link  n) . 

The  base  coordinates  are  defined  as  the  Oth  coordinate  frame 

(z  ,y  ,z  )  which  is  also  the  inertial  robot  arm  coordinate  frame, 
o   o   o 

Every  coordinate  frame  is  determined  and  established  by  three 
rul e  s : 

1.  the  z.  „  axes  lies  along  the  motion  axis  of  ith  joint 

i~*l 

2.  the  x.  axis  is  normal  to  the  z   1  axis,  pointing  away 

3.  the  y.  axis  completes  the  righthand  coordinate  system 
The  location  of  coordinate  frame  0  may  be  choosen  anywhere  in 

supporting  base,  as  long  as  the  z   axis  lies  along  the  motion  axis  of 

first  joint.   The  nth  frame  can  be  placed  anywhere  in  hand  as  long  as 

the  x   axis  is  normal  to  the  z   „  axis, 
n  n-1 

The  Denavit-Hartenberg  representation  of  rigid  links  depend  on 
four  geometric  quantities  associated  with  each  link  that  completely 
describe  any  revolute/prismatic  joint. 


1.  6.  =  joint  angle  from  x._1  axis  to  x.  axis  about  z._1 

axis  (Right  Hand  Rule) 

2.  d.  =  distance  from  (i-l)th  coordinate  frame  origin  to 

intersection  of  z    axis  with  the  x   axis  along 
the  z   1  axis. 

3.  a.  =  offset  distance  from  intersection  of  z._1  axis  with 

x.  axis  to  origin  of  ith  frame  along  x.  axis  (or 

i.  l 

shortest  distance  between  z.  „  and  z.  axes. 

4.  a  =  offset  angle  from  z._1  axis  to  z   axis  about  x. 

axis  (Right  Hand  Rule) 
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For   a   rotary  joint,    d.,    a.,    and  o.    are   joint   parameters   and 

11       1 

remain  constant  for  a  robot.   6.  is  a  joint  variable  that  changes 

when  link  i  moves  (rotates)  with  link  i-1.   For  a  prismatic  joint, 

6.,  a.,  and  a,  are  joint  parameters  and  remain  constants  for  a  robot, 
11       i 

d.  is  joint  variable. 


PROCEDURE  FOR  ESTABLISHING  CONSISTENT  ORTHONORMAL  COORDINATE  SYSTEMS 

Given:  H-degree-of-freedom  robot  arm 

Establish:  an  orthonormal  coordinate  system  to  each  robot  arm  link. 

1.  Establish  base  coordinate  system. 

Establish  a  righthand  orthonormal  coordinate  system  (x  ,y  ,z  ) 

o   o   o 

at  supporting  base  with  z   axis  lying  along  joint  1  motion  axis. 

2.  Initialize  and  loon. 

For  each  i,  i=l,...n,  perform  steps  3-6 


3.  Establish  joint  axis. 

Align  z.  with  joint  i+1  motion  axis 

4.  Establish  origin  of  ith  coordinate  system. 

Locate  origin  of  ith  coordinate  system  at  intersection  of  z.  and 

z.  „  axes  or  at  interestion  of  common  normals  between  z.  and  z.  „ 
i-1  i       l-l 

axes  and  z.  axis. 

l 
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5.  Establish  _x._  axis, 

-  ±<zi-l  x  zi) 
Establish  x.  =  

1    llz.  ,  X  z.M 
1-1    1 

or  along  common  normal  between  z,_1  and  z.  axes  when  they  are 
parallel. 

6.  Establish  y._  axis. 

-  (z.  x  x.) 

Establish  y.  =  

1    II*.  x  x.M 
i    l 

to  complete  righthand  coordinate  system. 

7.  Find  joint  and  lint  parameters. 

For  each  i,  i=l,...,n,  perform  steps  8-11. 


8.  Find  d. . 
^ 

d   is  distance  from  (i-l)th  coordinate  system  origin  to 

intersection  of  z .  .  axis  and  x.  axis  along  z.  „  axis.   It  is 
l-l  l  l-l 

joint  variable  if  joint  i  is  prismatic. 


9.  Find  a. . 
.^ 

a.  is  distance  from  intersection  of  z.  ,  axis  and  x.  axis  to  ith 
l  l-l  l 

coordinate  system  origin  along  x.  axis. 


10.  Find  6. . 
.^ 

6.  is  rotation  angle  from  x.  „  axis  to  x.  axis  about  z.  „  axis, 
l  l-l  l  l-l 

It  is  joint  variable  if  joint  i  is  rotary. 

11.  Find  a. . 

^L 

a.  is  rotation  angle  from  z,  .,  axis  to  z,  axis  about  x.  axis, 
l  i-1  i  l 
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Once  the  Denavit-Hartenberg  representation  coordinate  system  is 
established  for  each  link: 

A  Homogeneous  tranformation  matrix,  A.  1,   can  be  developed 

(Relates  ith  coordinate  frame  to  (i-l)th  coordinate  system) 

1.  rotate  about  z.  4  axis  an  angle  of  0.  to  align  x.  , 

axis  with  x.  axis  (x.  4  axis  is  parallel  to  x.) 
l        l-l  l 

2.  translate  along  x.  -  axis  a  distance  of  d   to  bring 

x,  «  and  x.  axes  into  coincidence. 
i-1      l 

3.  translate  along  x.  axis  a  distance  of  a.  to  bring 

l  l 

two  origins  into  coincidence 

4.  rotate  about  x.  axis  an  angle  of  a.  to  bring  the  two 

l  l 

coordinate  systems  in  coincidence. 
Each  operation  is  expressed  by  a  basic  homogeneous  rotation  or 
translation  matrix,  and  the  product  of  4  basic  homogeneous 
transformation  matrices 

This  yields  composite  homogenous  tranformation  matrix.  A.*, 
known  as  the  Denavit-Hartenberg  transformation  matrix  for  adjacent 
coordinate  frames. 
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APPENDIX  C 


A.   COMPUTER  PROGRAMS 

C********* ***************** ************************************** *majn 
C************** *************************** ************************majn 

£*****  ***********************  ***************************** ********MAJN 

COMMON  /ROB1/  G,PI,D2R,R2D 

COMMON  /ROB2/  ERTIA(4,4,15) ,RBAR(4,15) 

COMMON  /ROB3/  SMALLA(15) ,SMALLD(15) ,ALPHA(15) 

COMMON  /ROB4/  B2XX(15) ,B2YY(15) ,B2ZZ(15) 

COMMON  /ROB5/  AMASS ( 15 ) ,ACTIA( 15) , LINKS 

COMMON  /ROB6/  XBARU5)  ,YBAR(15)  ,ZBAR(15) 

COMMON  /ROB7/  GRAV(4,15) ,LNKTYP(15) 

COMMON  /ROB8/  A(4,4,15) ,PA(4,4,15) ,PPA(4,4,15) 

COMMON  /ROB9/  ZERO(15) ,TT(4,4,15) ,PTQ<4,4,15) 

COMMON  /ROB10/  POS(15,100) ,VEL(15,100) ,ACC(15,100) 

COMMON  /ROB11/  DELTA(15,100) ,DELTV(15,100) ,DELTP(15,100) 

COMMON  /ROB12/  SIMERR(15) , TOTACC(15) ,ACCSIM(15,100) , ISTEP,KNOTS 

COMMON  /ROB14/  POSSIM(15,100) ,ISIM 

REAL  T(15),TD(15),TDD(15),TAU(15)#FMK(15),THETA(15,100),THETD(15,1 
100),THDT2(15,100),TU(15,100),TIME(100),TS(15),TDS(15),TDDS(15),PAY 
2LD 

INTEGER  I, J, LINKS 

ISTEP=5 

ISIM=1 

CALL  CONST 
C LOAD  TRAJECTORY  KNOTS 

CALL  TRAJ  (KNOTS, TIME, THETA#THETD#THDT2,TU) 
C INITIALIZE  PASS  ON  MATRICES 

DO  10  1=1,15 

T(I)=0.0 

TD(I)=0.0 

TDD(I)=0.0 

FMK(I)=0.0 
10    CONTINUE 
C CALL  SUBROUTINE  TO  COMPUTE  ACCELERATIONS 

DO  30  J=l, KNOTS 

PAYLD=0.0 

DO  20  1=1, LINKS 

FMK(I)=0.0 

TAU(I)=TU(I,J) 

T(  I) =THETA( I, J) *D2R 

TD(I)=THETD(I,J)*D2R 

TDD( I) =THDT2( I, J) *D2R 
20    CONTINUE 

CALL  ROBARM  (J,T,TD,TDD,TAU,FMK,PAYLD,TIME) 
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CALL  OUTPUT  (J,T,TD,TDD,THETA,THETD,THDT2,TIME,PAYLD) 
30    CONTINUE 
C FrND  MODEL  ERRORS 

CALL  ERRORS  ( KNOTS, THETA,THETD,THDT2) 

c FIND  SIMULATION  ERRORS 

STOP 
END 


C***** ************************************************** ****** ***ROBARM 
C* **************** ************************ ******************** ***Rog^RM 
C*****  ********  ***********************************************  *'***ROB  ARM 

SUBROUTINE  ROBARM  (KN,T,TD, TDD, TAU,FMK,PAILD, TIME) 

COMMON  /ROB1/  G,PI,D2R,R2D 

COMMON  /ROB2/  ERTIA(4,4,15) ,RBAR(4,15) 

COMMON  /ROB3/  SMALLA(15) ,SMALLD(15) ,ALPHA(15) 

COMMON  /ROB4/  B2XX(15) ,B2YY(15) ,B2ZZ(15) 

COMMON  /ROB5/  AMASS(15) ,ACTIA(15) , LINKS 

COMMON  /ROB6/  XBAR(15) , YBAR(15) ,ZBAR(15) 

COMMON  /ROB7/  GRAV(4,15) ,LNKTYP(15) 

COMMON  /ROB8/  A(4,4,15) ,PA(4,4,15) ,PPA(4,4,15) 

COMMON  /ROB9/  ZERO(15) ,TT(4,4,15) ,PTQ(4,4,15) 

COMMON  /ROB10/  POS(15,100) ,VEL(15,100) ,ACC(15,100) 

COMMON  /ROB14/  POSSIM(15,100) ,ISIM 

REAL  T(15),TD(15),TDD(15),TAU(15)»FMK(15),HJ(15),B(15),EJ(15),H(15 
1,15) ,WORK(45,3) ,TIME(100) ,TDOT2(100) ,TDOT(100) ,TPOS(100) ,PAYLD 

INTEGER  I,J,ISTAT,KN 

CALL  KINEMT  (T,PAILD) 

DO  10  1=1,15 

B(I)=0.0 
10    CONTINUE 

CALL  ARM1  (T,TD,ZERO,FMK,B) 
c L0AD  ej  VECTOR 

DO  40  1=1, LINKS 

DO  20  J=l, LINKS 

HJ(J)=0.0 

EJ(J)=0.0 
20    CONTINUE 

EJ(I)=1.0 

CALL  ARM2  (T,EJ,HJ) 

DO  30  J=l, LINKS 

H(J,I)=BJ(J) 
30    CONTINUE 
40    CONTINUE 

DO  50  1=1, LINKS 

TDD(I)=TAU(I)-B(I) 
50    CONTINUE 

IER=0 
C COMPUTE  LINK  ACCELERATIONS 

CALL  TDDSOL  (H, 15, LINKS, LINKS, TDD, 15, 1,0, WORK, IER) 


134 


-LOAD  VECTORS    INTO  PERM  MATRICES 


DO   80  LN=1, LINKS 

TDD(LN) =TDD(LN) *R2D 

ACC(LN,KN)=TDD(LN) 

DO  60   1=1,  KN 

TDOT2(I)=ACC(LN,I) 

TDOT(I)=0.0 
60  CONTINUE 

CALL  VELSOL    (TIME,TD0T2,TD0T,KN) 
c _ CALCULATE  VELOCITY 

VEL(LN, KN) =TDOT( KN) 

TD(LN)=TDOT(KN) 

DO  70   1=1, KN 

TDOT(I)=VEL(LN,I) 
70    CONTINUE 
C     CALL  POSSOL  (TIME,TDOT,TDOT2,TPOS,KN) 

CALL  VELSOL  (TIME,TDOT,TPOS,KN) 
c CALCULATE  POSITION 

POS (LN, KN) =TPOS ( KN) 

IF  (KN.EQ.l)  TINIT=T(LN) *R2D 

T(LN) =TPOS ( KN) +TINIT 
80    CONTINUE 
C CALL  SIMULATION 

IF  (ISIM.NE.l)  GO  TO  90 

CALL  ROBSIM  (KN,T,TD, TDD, TAU,FMK,PAYLD, TIME) 
90    CONTINUE 
RETURN 
END 
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c**********************************  ***************************  ♦♦***ARJM1 

£** ******************************************************* *********pjUAl 

(^*  ************  **************************************************  ***ARM1 

SUBROUTINE  ARM1    (T,TD,TDD,FMK,TAU) 

COMMON   /ROB1/   G,PI,D2R,R2D 

COMMON   /ROB2/    ERTIA(4,4,15) ,RBAR(4,15) 

COMMON  /ROB3/    SMALLAU5)  ,SMALLD(15) ,  ALPHA(15) 

COMMON   /ROB4/   B2XX(15) ,B2YY(15) ,B2ZZ(15) 

COMMON  /ROB5/   AMASS ( 15 ) ,ACTIA( 15) , LINKS 

COMMON   /ROB6/    IBAR(15) , YBAR(15) ,ZBAR(15) 

COMMON  /ROB7/   GRAV(4,15) ,LNKTYP(15) 

COMMON  /ROB8/   A(4,4,15) ,PA<4,4,15) ,PPA(4,4,15) 

COMMON  /ROB9/   ZERO(15) ,TT(4,4,15) ,PTQ(4,4,15) 

REAL  T(15),TD(15),TDD(15),TAU(15),FMK(15),DD(4,4,15),CC(4,15),C1(4 
1,4,15),C2(4,4,15),C3(4,4,15),C4(4,4,15),C5(4,4,15),C6(4,4,15),C7(4 
2,4,15),C8(4,4,15),C9(4,4,15),C10(4,4,15),C11(4,4,15),C12(4,4,15),C 
313(4,4,15),C14(4,15),C15(4,15),C17,C19,TD1(4,4,15),TD2(4,4,15) 

INTEGER  I, J 
C INITALIZE  ALL   CONSTANT  MATRICES 

DO  30   1=1,4 

DO  20  J=l,4 

CC(I,J)=0.0 

C14(I,J)=0.0 

C15(I,J)=0.0 

DO  10   K=l, LINKS 

DD(I,J,K)=0.0 

TD1(I,J,K)=0.0 

TD2(I,J,K)=0.0 

C1(I,J,K)=0.0 

C2(I,J,K)=0.0 

C3(I,J,K)=0.0 

C4(I,J,K)=0.0 

C5(I,J,K)=0.0 

C6(I,J,K)=0.0 

C7(I,J,K)=0.0 

C8(I,J,K)=0.0 

C9(I,J,K)=0.0 

C10(I,J,K)=0.0 

C11(I,J,K)=0.0 

C12(I,J,K)=0.0 

C13(I,J,K)=0.0 
10  CONTINUE 

20  CONTINUE 

30  CONTINUE 
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T  = 
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J-l  J 


J-l 


P  A 
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P  Q 
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CI 


C3 


40 

50 
60 
C 

C 

c 
c 

C   J 

c 

c 

c 

c 


TD1/TD2  MATRICES  INITIALIZED  TO  ZERO  IN  CONST  SUBROUTINE 

DO  60  J=l, LINKS 

IF  (J.EQ.l)  GO  TO  40 

CALL  MAT44  (TD1,J-1,A,J,C1,J) 

CALL  MAT44  (TT,J-1,PA,J,C2, J) 

CALL  MATC4  (TD(J) , C2,J, C3, J) 

CALL  ADD44  (C1,J,C3, J,TD1,J) 

GO  TO  50 

CONTINUE 

CALL  MATC4  (TD(J) ,PA,J,TD1,J) 

CONTINUE 

CONTINUE 
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80 
90 


—LOAD  T  MATRIX 


•  •      ••  •      *A# 

T  =  T    A  +  2  T    J  Q 

J     J-l  J      J-l  P  Q   J 


J-l 


C4 


C7 
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P  A 
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P  A 
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P  Q 
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Cll 


DO  90  J=l, LINKS 

IF  (J.EQ.l)  GO  TO  70 

CALL  MAT44  (TD2, J-l, A,J,C4, J) 

CALL  MATC4  (2.0,TD1,J-1,C5,J) 

CALL  MAT44  ( C5, J, PA, J, C6, J) 

CALL  MATC4  (TD(J) , C6,J, C7, J) 

CALL  MAT44  (TT, J-1,PPA, J,C8,J) 

CALL  MATC4  (TD(J) **2, C8,J, C9, J) 

CALL  MAT44  (TT, J-l, PA, J, CIO, J) 

CALL  MATC4  (TDD(J) ,C10,J, Cll, J) 

CALL  ADDALL  (C4,J,C7,J,C9,J, Cll, J,TD2,J) 

GO  TO  80 

CONTINUE 

CALL  MATC4  (TD(J) **2,PPA,J, C9,J) 

CALL  MATC4  (TDD(J) ,PA,J, Cll, J) 

CALL  ADD44  ( C9, J, Cll, J,TD2, J) 

CONTINUE 

CONTINUE 
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C LOAD  CONSTANT  MATRICES 

DO  140  11=1, LINKS 

1=7-11 
c L0AD  CONSTANT  D  MATRIX 

C  ..T 

C   D=JT   +A   D 

C    I    I   I      1+1  1+1 

C      =  C12   +    C13 

IF  (I.EQ.LINKS)  GO  TO  100 

CALL  MAT44T  (ERTIA,  I,TD2, 1,C12, 1) 

CALL  MAT44  (A, I+1,DD, 1+1, C13,I) 

CALL  ADD44  (C12, I, C13, I,DD, I) 

GO  TO  110 
100   CONTINUE 

CALL  MAT44T  (ERTIA,I,TD2, 1,DD,  I) 
110   CONTINUE 
c L0AD  CONSTANT  C  MATRIX 

C  I 

CC=MR+AC 
CI  II  1+1    1+1 

C 

C  C14  +       C15 

c 

IF  (I.EQ.LINKS)  GO  TO  120 
CALL  MATC1  (AMASS, I,RBAR, I, C14, I) 
CALL  MAT41  (A, 1+1, CC, I+1,C15, I) 
CALL  ADD11  (C14,I,C15,I,CC,I) 
GO  TO  130 
120   CONTINUE 

CALL  MATC1  (AMASS,  I,RBAR,  I,  CC,  I) 
CONTINUE 
CONTINUE 
L0AD  PARTIAL  T  WRT  Q 

C   P  T  P  A    J 

C J   =  T     J  T 

C   P  Q       J-l   P  Q    J 
C      J  J 

C 

DO  190  J=l, LINKS 

IF  (J.EQ.l)  GO  TO  150 

CALL  MAT44  (TT,J-1,PA,J,PTQ,J) 

GO  TO  180 
150   CONTINUE 

DO  170  11=1,4 

DO  160  JJ=1,4 

PTQ(II,JJ,J)=PA(II,JJ,J) 
160   CONTINUE 
170   CONTINUE 
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180        CONTINUE 
190        CONTINUE 

. PAT  P     ETJOPTh /TADflTTl? 

C                                1    P  T 

C        F        =     TR    1 1 

CI                    1    P  Q 
C                                1           I 

1 
D      1      - 
I    1 

1 

—————     CAL.C    rUKLH/  lUKUUt 

T     P  T 

G        1      C 

P  Q          I 

I 

C                =                   C17 

- 

CI  9 

DO  200   1=1, LINKS 
CALL  FIRST   (PTQ, I,DD, C17) 
CALL  SECOND   (GRAV,PTQ, I, CC, CI 9) 
TAU(I)=C17-C19 
TAU( I) =TAU( I) /10000. 0 
200        CONTINUE 
RETURN 
END 
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C* ************************************ ******************** ********  * ARM2 
C** *********************************************************** *****Arm2 
C********************************************* *************** ******A£M2 

SUBROUTINE  ARM2  (T,TDD,TAU) 

COMMON  /ROB1/  G,PI,D2R,R2D 

COMMON  /ROB2/  ERTIA(4,4,15) ,RBAR(4,15) 

COMMON  /ROB3/  SMALLA(15) ,SMALLD(15) ,ALPHA(15) 

COMMON  /R0B4/  B2XX(15) ,B2YY(15) ,B2ZZ(15) 

COMMON  /ROB5/  AMASS ( 15 ) ,ACTIA( 15) , LINKS 

COMMON  /ROB6/  XBAR(15) , YBAR(15) ,ZBAR(15) 

COMMON  /ROB7/  GRAV(4,15) ,LNKTYP(15) 

COMMON  /ROB8/   A(4,4,15) ,PA(4,4,15) ,PPA(4,4,15) 

COMMON  /ROB9/   ZERO(15) ,TT(4,4,15) ,PTQ(4,4,15) 

REAL  T(15),TDD(15),TAU(15),DD(4,4,15),C4(4,4,15),C10(4,4,15),C11(4 
l,4,15),a2(4,4,15),C13(4,4,15),TD2(4,4,15),C17 

INTEGER  I, J 
C INITALIZE  CONSTANT  MATRICES 

DO  30  1=1,4 

DO  20  J=l,4 

DO  10  K=l, LINKS 

DD(I,J,K)=0.0 

TD2(I,J,K)=0.0 

C4(I,J,K)=0.0 

C10(I,J,K)=0.0 

aid, J, K)  =0.0 

C12(I,J,K)=0.0 

C13(I,J,K)=0.0 
10    CONTINUE 
20    CONTINUE 
30    CONTINUE 
C 

C— 
C 
C 
C 
C 
C 

C- 
C 
C- 


T  = 
J 


T    A  + 
J-l  J 


LOAD  STRIPPED  T  MATRIX 

PA 

T    J  Q 

J-l  P  Q    J 
J 


C4 


Cll 


DO  60  J-l, LINKS 

IF  (J.EQ.l)  GO  TO  40 

CALL  MAT44  (TD2,J-1,A,J, C4,J) 

CALL  MAT44  (TT,J-1,PA,J,C10, J) 

CALL  MATC4  (TDD(J) , CIO, J, Cll, J) 

CALL  ADD44  (C4,J,C11,J,TD2,J) 

GO  TO  50 

40    CONTINUE 

CALL  MATC4  (TDD(J) ,PA, J,TD2, J) 

50    CONTINUE 
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CONTINUE 
LOAD  CONSTANT  D  MATRIX 


C      =   C12   +    C13 
c 


DO  90  11=1, LINKS 

1=7-11 

IF  (I.EQ.LINKS)  GO  TO  70 

CALL  MAT44T  (ERTIA,I,TD2,I, C12, I) 

CALL  MAT44  (A, I+1,DD, 1+1, C13, 1) 

CALL  ADD44  (C12,I,  C13, I,DD,  I) 

GO  TO  80 

70    CONTINUE 

CALL  MAT44T  (ERTIA,  I,TD2, 1,DD,I) 

80    CONTINUE 

90    CONTINUE 

c QALC   FORCE/ TORQUE 

C  I  P  T 

C   F   =  TR  I  1  D   I 

C    I        I  P  Q    I  I 

C  III 


C 

C       =        C17 


DO  100  1=1, LINKS 
CALL  FIRST  (PTQ, I,DD, C17) 
TAU(I)=C17 

TAU(I)=TAU(I)/10000.0 
100   CONTINUE 


C— 


RETURN 
END 
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C************************* ************************************ ****0UTPUT 

£*  ****************  **********************************  **************Q|J'ppjj'p 

C***************** ******************** ****************************0UTPUT 

SUBROUTINE  OUTPUT   ( J , T, TD, TDD, THETA, THETD, THDT2 , TIME, PAYLD) 

COMMON  /ROB1/   G,PI,D2R,R2D 

COMMON   /ROB2/    ERTIA(4,4,15) ,RBAR(4,15) 

COMMON  /ROB3/    SMALLA(15) ,SMALLD(15) , ALPHA(15) 

COMMON  /ROB4/   B2XX(15) ,B2YY(15) ,B2ZZ(15) 

COMMON  /ROB5/   AMASS(15) ,ACTIA(15) , LINKS 

COMMON  /ROB6/    XBAR(15) , YBAR(15) ,ZBAR(15) 

COMMON  /ROB7/   GRAV(4,15) ,LNKTYP(15) 

COMMON  /ROB8/    A(4,4,15) ,PA(4,4,15) ,PPA(4,4,15) 

COMMON  /ROB9/   ZERO(15) ,TT(4,4,15) ,PTQ(4,4,15) 

COMMON  /ROB10/    POS(15,100) ,VEL(15,100) ,ACC(15,100) 

COMMON  /ROB11/    DELTA(15,100) ,DELTV(15,100) ,DELTP(15,100) 

COMMON  /ROB12/    SIMERR(15) ,TOTACC(15) ,ACCSIM(15,100) , ISTEP,KNOTS 

COMMON  /ROB13/   T2(15,100) ,TLAST(15) ,TDLAST(15) ,TDDLST(15) 

COMMON  /ROB14/    POSSIM(15,100) , ISIM 

REAL  T(15) ,TD(15) ,TDD(15) ,THETA(15,100) ,THETD<15,100) ,THDT2(15,100 
1),TIME(100), PAYLD 

INTEGER  I, J, KNOTS, ISIM 
C            WRITE   (27,40)    J,PAYLD,TIME(J) 
C ACCELERATION 

C  WRITE   (27,50) 

DO  10   K=l, LINKS 

DELTA( K, J ) =ABS ( THDT2 ( K, J ) -TDD( K) ) 
C            WRITE   (27,80)    K,THDT2(K,J) ,TDD(K) ,DELTA(K,J) 
10           CONTINUE 
c VELOCITY 

C  WRITE   (27,60) 

DO  20   K=l, LINKS 

DELTV ( K,  J )  =ABS  ( THETD(  K,  J ) -TD(  K) ) 
C  WRITE   (27,80)    K,THETD(K,J) ,TD(K) ,DELTV(K,J) 

20  CONTINUE 

C POS ITION 

C  WRITE   (27,70) 

DO  30   K=l, LINKS 

DELTP( K, J ) =ABS ( THETA ( K, J ) -T( K) ) 
C  WRITE   (27,80)    K,THETA(K,J) ,T(K) ,DELTP(K,J) 

30  CONTINUE 

IF   (ISIM.EQ.l)    GO  TO  40 
C OUTPUT  TO  GRAPHICS   DATA  FILES 

WRITE   (41,80)    TIME(J),THDT2(1,J),TDD(1) 

WRITE   (42,80)    TIME(J) ,THDT2(2, J) ,TDD(2) 

WRITE   (43,80)    TIME(J) ,THDT2(3,J) ,TDD(3) 

WRITE   (44,80)    TIME(J) ,THDT2(4, J) ,TDD(4) 

WRITE   (45,80)    TIME(J) ,THDT2(5, J) ,TDD(5) 

WRITE   (46,80)    TIME(J) ,THDT2(6, J) ,TDD(6) 
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c — 


C— 

40 


50 


70 


40 


WRITE  (31,80)  TIME(J),THETD(1,J),TD(1) 

WRITE  (32,80)  TIME(J) ,THETD(2,J) ,TD(2) 

WRITE  (33,80)  TIME(J) ,THETD(3,J) ,TD(3) 

WRITE  (34,80)  TIME(J) ,THETD(4,J) ,TD(4) 

WRITE  (35,80)  TIME(J) ,THETD(5, J) ,TD(5) 

WRITE  (36,80)  TIME(J) ,THETD(6, J) ,TD(6) 

WRITE  (21,80)  TIME(J),THETA(1,J),T(1) 

WRITE  (22,80)  TIME(J) ,THETA(2,J) ,T(2) 

WRITE  (23,80)  TIME(J) ,THETA(3, J) ,T(3) 

WRITE  (24,80)  TIME(J) ,THETA(4,J) ,T(4) 

WRITE  (25,80)  TIME(J) ,THETA(5, J) ,T(5) 

WRITE  (26,80)  TIME(J) ,THETA(6,J) ,T(6) 

GO  TO  70 

CONTINUE 

IF  (J.NE.KNOTS)  GO  TO  70 

WRITE  (27,90)  ISTEP, KNOTS 

DO  50  11=1, LINKS 

SIMERR( II) =SIMERR( II) /TOTACC( II) 

WRITE  (27,100)  II,SIMERR(II) 

CONTINUE 

WRITE  (21,110) 

(22,110) 

(23,110) 

(24,110) 

(25,110) 

(26,110) 


TIME(J) ,THETA(1, J) ,POSSIM(l, J) 
TIME(J),THETA(2,J),POSSIM(2,J) 
TIME(J),THETA(3,J),POSSIM(3,J) 
TIME(J) ,THETA(4,J) ,POSSIM(4,J) 
TIME(J),THETA(5,J),POSSIM(5,J) 
TIME(J) ,THETA(6,J) ,POSSIM(6,J) 


WRITE 
WRITE 
WRITE 
WRITE 
WRITE 
CONTINUE 
RETURN 

FORMAT  (/13HKNOT  NUMBER  =,I5,10H  PASLOAD  =,F15.7,8H  TIME  =  ,F13.5 
1) 

(3F15.7) 

(/17HNUMBER  OF  STEPS  =,I5,18H  KNOTS  PROCESSED  =,15) 

(/13HLINK  NUMBER  =,I4,10H  ERROR  =  ,E18.8) 

(3F15.7) 


80 

FORMAT 

90 

FORMAT 

100 

FORMAT 

110 

FORMAT 

END 
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C*********** ****************************** ******************** ***C0NST 

£*************  **********************************************  *****CQfv|g'J- 

£  ***************************************************  *************^QfvJg  J 

SUBROUTINE  CONST 
COMMON  /ROB1/  G,PI,D2R,R2D 
COMMON  /ROB2/  ERTIA(4,4,15) ,RBAR(4,15) 
COMMON  /ROB3/  SMALLA(15) ,SMALLD(15) ,ALPHA(15) 
COMMON  /ROB4/  B2XX(15) ,B2YY(15) ,B2ZZ(15) 
COMMON  /ROB5/  AMASS(15) , ACTIA(15) , LINKS 
COMMON  /ROB6/  XBAR(15) , YBAR(15) ,ZBAR(15) 
COMMON  /ROB7/  GRAV(4,15) ,LNKTYP(15) 
COMMON  /ROB8/  A(4,4,15) ,PA(4,4,15) ,PPA(4,4,15) 
COMMON  /ROB9/  ZEROU5)  ,TT(4,4,15)  ,PTQ(4,4,15) 
C*********************************L0AD  UNIVERSAL  CONSTANT 

PILARS IN(1.0)*2.0 
D2R=PI/ 180.0 
R2D=180.0/PI 
G=980.621 
C*********************************L0AD  ARM  CHARACTERISTICS 

READ  (15,*)  LINKS,(LNKTYP(I),SMALLA(I)>SMALLD(I),ALPHA(I),B2XX(I), 
1B2YY( I) ,B2ZZ( I) ,XBAR( I) , YBAR( I) ,ZBAR( I) , AMASS( I) ,  ACTIA( I) , 1=1, LINK 
2S) 
c  ************************* ««*****LOAD  LINK  INERTIA'S 

DO  10  1=1,  LINKS 

ERTIA(1,1, I)=AMASS( I) *( (B2YY( I) +B2ZZ( I)-B2XX( I) ) /2.0) 

ERTIA(2,2,I)=AMASS(I)*((B2XX(I)+B2ZZ(I)-B2YY(I))/2.0) 

ERTIA(3,3, I) =AMASS( I) *( (B2XX( I) +B2YY( I)-B2ZZ( I) ) /2.0) 

ERTIA( 1, 4, I) =AMASS( I) *XBAR( I) 

ERTIA(2, 4, I)=AMASS( I) *YBAR( I) 

ERTIA(3,4, I)=AMASS( I) *ZBAR( I) 

ERTIA(4,1, I)=AMASS( I) *XBAR( I) 

ERTIA( 4 , 4 , 1) =AMASS ( I)  *1 . 0 

ERTIA(4,1, I)=AMASS( I) *XBAR( I) 

ERTIA(4, 2, I) =AMASS( I) *YBAR( I) 

ERTIA( 4 , 3 , I ) =AMASS ( I ) *ZBAR( I ) 

ERTIA(1,2,I)=0.0 

ERTIA(1,3,I)=0.0 

ERTIA(2,1,I)=0.0 

ERTIA(2,3,I)=0.0 

ERTIA(3,1,I)=0.0 

ERTIA(3,2,I)=0.0 
10    CONTINUE 
c  ********************************FIND  Q  maxrjx  p  LINK  MASS  CENTERS 

DO  30  1=1,4 

DO  20  J=l,  LINKS 

RBAR(I,J)=0.0 
20    CONTINUE 
30    CONTINUE 

RBAR(3,1)=ZBAR(1) 

RBAR(4,1)=1.0 
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RBAR(1,2)=XBAR(2) 
RBAR(3,2)=ZBAR(2) 
RBAR(4,2)=1.0 
RBAR(3,3)=ZBAR(3) 
RBAR(4,3)=1.0 
RBAR(2,4)=YBAR(4) 
RBAR(4,4)=1.0 
RBAR(3,5)=ZBAR(5) 
RBAR(4,5)=1.0 
RBAR(3,6)=ZBAR(6) 
RBAR(4,6)=1.0 
C*********************************ZER0  AND  ALPHA~>RAD  MATRICES 

DO  39  1=1,15 
ZERO(I)=0.0 

39  CONTINUE 

DO  40  1=1, LINKS 
ALPHA( I) =ALPHA( I) *D2R 

40  CONTINUE 
C******«************************INITIALIZE  A  MATRICES 

DO  80  J=l,4 

DO  70  K=l,4 

IF  (J.EQ.K)  GO  TO  50 

A(J,K,1)=0.0 

TT(J,K,1)=0.0 

GO  TO  60 
50    CONTINUE 

A(J,J,1)=1.0 

TT(J,J,1)=1.0 
60    CONTINUE 
70    CONTINUE 
80    CONTINUE 

RETURN 

END 


145 


Q     ************************************************* **************^J^£J^£ 

q  ********************************************************* ******KINEMT 
q  ************************************************************ ***KINEMT 

SUBROUTINE  KINEMT  (T,PAYLD) 

COMMON  /ROB1/  G,PI,D2R,R2D 

COMMON  /ROB2/  ERTIA(4,4,15) ,RBAR(4,15) 

COMMON  /ROB3/  SMALLA(15) ,SMALLD(15) , ALPHA(15) 

COMMON  /R0B4/  B2XX(15) ,B2YY(15) ,B2ZZ(15) 

COMMON  /ROB5/  AMASS ( 15 ) ,ACTIA( 15) , LINKS 

COMMON  /ROB6/  XBAR(15) , YBAR(15) ,ZBAR(15) 

COMMON  /ROB7/  GRAV(4,15) ,LNKTYP(15) 

COMMON  /ROB8/  A(4,4,15) ,PA(4,4,15) ,PPA(4,4,15) 

COMMON  /ROB9/  ZERO(15) ,TT(4,4,15) ,PTQ(4,4,15) 

REAL  T(15),PAILD 
C COMPUTE  INDIVIDUAL  LINK  A-MATRICES 

DO  10  1=1,  LINKS 

A(l,l,I)=COS(T(I)) 

A(l,2,I)=-COS(ALPHA(I))*SIN(T(I)) 

A(1,3,I)=SIN(ALPHA(I))*SIN(T(I)) 

A(l,4,I)=SMALLA(I)*COS(T(I)) 

A(2,1,I)=SIN(T(I)) 

A(2,2,I)=COS(ALPHA(I))*COS(T(I)) 

A(2,3,I)=-SIN(ALPHA(I))*COS(T(I)) 

A(2,4,I)=SMALLA(I)*SIN(T(I)) 

A(3,1,I)=0.0 

A(3,2,I)=SIN(ALPHA(I)) 

A(3,3,I)=COS(ALPHA(I)) 

A(3,4,I)=SMALLD(I) 

A(4,1,I)=0.0 

A(4,2,I)=0.0 

A(4,3,I)=0.0 

A(4,4,I)=1.0 
10    CONTINUE 
C COMPUTE  AND  LOAD  1ST  PARTIAL  A-MATRIX 

DO  30  1=1,  LINKS 

PA(1,1,I)=-SIN(T(D) 

PA(l,2,I)=-COS(T(I))*COS(ALPHA(D) 

PA(l,3,I)=COS(T(I))*SIN(ALPHA(D) 

PA(1,4,I)=-SMALLA(I)*SIN(T(D) 

PA(2,l,I)=COS(T(D) 

PA(2,2,I)=-SIN(T(I))*COS(ALPHA(I)) 

PA(2,3, 1)=SIN(T(  I) )  *SIN(ALPHA(  I) ) 

PA(2,4,I)=SMALLA(I)*COS(T(D) 

DO  20  K=l,4 

PA(3,K,I)=0.0 

PA(4,K,I)=0.0 
20    CONTINUE 
30    CONTINUE 
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COMPUTE  AND  LOAD  2ND  PARTIAL  A-MATRIX 

DO  50  1=1, LINKS 

PPA(l,l,I)=-COS(T(I)) 

PPA(l,2,I)=SIN(T(I))*COS(ALPHA(I)) 

PPA(1,3,I)=-SIN(T(I))*SIN(ALPHA(I)) 

PPA(l,4,I)=-SMALLA(I)*COS(T(D) 

PPA(2,1,I)=-SIN(T(I)) 

PPA(2,2,I)=-COS(T(I))*COS(ALPHA(I)) 

PPA(2,3,I)=COS(T(I))*SIN(ALPHA(I)) 

PPA(2,4,I)=-SMALLA(I)*SIN(T(I)) 

DO  40  K=l,4 

PPA(3,K,I)=0.0 

PPA(4,K,I)=0.0 
40    CONTINTJE 
50    CONTINUE 
C L0AD  LINK  TRANSPOSITION  T-MATRICES 

DO  110  1=1, LINKS 


DO  100  J=l,4 

DO  90  K=l,4 

TT(J,K,I)=0.0 

DO  80  L=l,4 

IF  (I.EQ.l)  GO  TO  60 

TT(J,K, I)=TT(J,K, I) +TT(J,L, 1-1) *A(L,K, I) 

GO  TO  70 
60    CONTINUE 

TT(J,K,1)=A(J,K,1) 
70    CONTINUE 
80    CONTINUE 
90    CONTINUE 
100   CONTINUE 

110   CONTINUE 

c L0AD  GRAVITY  MATRIX 

DO  120  1=1, LINKS 
GRAV(1,I)=0.0 
GRAV(2,I)=0.0 
GRAV(3,I)=G 
GRAV(4,I)=0.0 
120   CONTINUE 
RETURN 
END 
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(>************** *********************  ********** *************** *****TRAJ 
C****** ********************************************************* **xraJ 
C**** ************************************************************ *XRAJ 

SUBROUTINE  TRAJ  (KNOTS, TIME, THETA,THETD,THDT2,TU) 

COMMON  /ROB5/  AMASS(15) , ACTIA(15) , LINKS 

COMMON  /ROB13/  T2(15,100) ,TLAST(15) ,TDLAST(15) ,TDDLST(15) 

COMMON  /ROB14/  POSSIM(15,100) ,  ISIM 

REAL  THETA(15,100) ,THETD(15,100) ,THDT2(15,100) ,TU(15,100) ,TIME(100 
1) 

INTEGER  I, J, KNOTS 

READ  (20,*)  KNOTS 

DO  20  1=1, LINKS 

DO  10  J=l, KNOTS 

READ  (20,*)  TIME(J),THETA(I,J),THETD(I,J),THDT2(I,J) 

T2(I,J)=THDT2(I,J) 
10    CONTINUE 
20    CONTINUE 

DO  30  J=l, KNOTS 

READ  (50,*)  TDUM 

READ  (50,*)  TU(1,J),TU(2,J),TU(3,J),TU(4,J),TU(5,J),TU(6,J) 
C    WRITE  (27,40)  TDUM,TU(1,J) ,TU(2,J) ,TU(3,J) ,TU(4,J) ,TU(5,J) ,TU(6,J) 
30    CONTINUE 
C    KNOTS=10 

RETURN 
C40   FORMAT  (7F10.4) 

END 
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q* ***************  ********************************************** ***ERRORS 
q*************  **********************************************  ******p;RRORS 
C***** ********  ************  ************************************ ****ERRORS 

SUBROUTINE  ERRORS  ( KNOTS, THETA,THETD,THDT2) 

COMMON  /ROB5/  AMASS ( 15 ) ,ACTIA( 15) , LINKS 

COMMON  /ROB11/  DELTA(15,100) ,DELTV(15,100) ,DELTP(15,100) 

COMMON  /ROB14/  POSSIM(15,100) , ISIM 

REAL  THETA(15,100),THETD(15,100),THDT2(15,100),DELA,DELV,DELP,DN,A 
1A, AV, AP, MINA, MINV, MINP, MAXA, MAXV, MAXP, TDDSUM, TDSUM, TSUM 
C INITIALIZE  VARIABLES 

DO  20  K=l, LINKS 

DELA=0.0 

DELV=0.0 

DELP=0.0 

TDDSUM=0.0 

TDSUM=0.0 

TSUM=0.0 

MAXA=0.0 

MAXV=0.0 

MAXP=0.0 

MINA=100.0 

MINV=100.0 

MINP=100.0 
c FIND  MAX/MIN 

DO  10  J=l,  KNOTS 

IF  (ABS(DELTA(K,J)).GT.MAXA)  MAXA=ABS( DELTA (K, J) ) 

IF  (ABS(DELTV(K,J)).GT.MAXV)  MAXV=ABS(DELTV(K,J) ) 

IF  (ABS(DELTP(K,J)).GT.MAXP)  MAXP=ABS(DELTP(K, J) ) 

IF  (ABS(DELTA(K,J)).LT.MINA)  MINA=ABS(DELTA(K,J) ) 

IF  (ABS(DELTV(K,J)).LT.MINV)  MINV=ABS(DELTV(K,J) ) 

IF  (ABS(DELTP(K,J)) .LT.MINP)  MINP=ABS(DELTP(K, J) ) 
c FIND  DIFFERENCE/ ERRORS 

DELA=DELA+ABS ( DELTA( K,  J ) ) 

DELV=DELV+ABS ( DELTV ( K,  J ) ) 

DELP=DELP+ABS ( DELTP( K,  J ) ) 

TDDSUM=TDDSUM+ABS ( THDT2 ( K,  J ) ) 

TDSUM=TDSUM+ABS ( THETD( K,  J ) ) 

TSUM=TSUM+ABS(THETA(K, J) ) 
10    CONTINUE 

AA=DELA/ TDDSUM 

AV=DELV/TDSUM 

AP=DELP/TSUM 

WRITE  (37,30)  AA, MINA, MAXA 

WRITE  (37,30)  AV, MINV, MAXV 

WRITE  (37,30)  AP, MINP, MAXP 
20    CONTINUE 
C-- 

RETURN 
30    FORMAT  (3F18.7) 

END 
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a******************************************************** *******ROBSIM 
****************** ******************** ************ ********** ****roBSIM 
******************************* *********************************^OBSIM 

SUBROUTINE  ROBSIM  (KN,T,TD, TDD, TAU,FMK,PAYLD, TIME) 

COMMON  /ROB1/  G,PI,D2R,R2D 

COMMON  /ROB2/  ERTIA(4,4,15) ,RBAR(4,15) 

COMMON  /ROB3/  SMALLA(15) ,SMALLD(15) ,ALPHA(15) 

COMMON  /ROB4/  B2XX(15) ,B2YY(15) ,B2ZZ(15) 

COMMON  /ROB5/  AMASS ( 15 ) ,ACTIA( 15) , LINKS 

COMMON  /ROB6/  XBAR(15) , YBAR(15) ,ZBAR(15) 

COMMON  /ROB7/  GRAV(4,15) ,LNKTYP(15) 

COMMON  /ROB8/  A(4,4,15) ,PA(4,4,15) ,PPA(4,4,15) 

COMMON  /ROB9/  ZERO(15) ,TT(4,4,15) ,PTQ(4,4,15) 

COMMON  /ROB10/  POS(15,100) ,VEL(15,100) #ACC(15,100) 

COMMON  /ROB12/  SIMERR(15) ,TOTACC(15) ,ACCSIM(15,100) , ISTEP,KNOTS 

COMMON  /ROB13/  T2(15,100) ,TLAST(15) ,TDLAST(15) ,TDDLST(15) 

COMMON  /ROB14/  POSSIM(15,100) ,VELSIM(15,100) ,ACLSIM(15,100) , ISIM 

REAL  T(15),TD(15),TDD(15),TAU(15),FMK(15),HJ(15),B(15),EJ(15),H(15 
1,15) ,WORK(45,3) ,TIME(100) ,TDOT2(100) ,TDOT(100) ,TPOS(100) ,TDDSIM(15 
2 ) , TDS IM( 1 5 ) , TS IM( 1 5 ) , TDOLD( 1 5 ) , DELTT, STEPSZ , PAYLD 

INTEGER  I,J,KN,LN 

IF  (KN.EQ. KNOTS)  GO  TO  120 

IF  (ISIM.NE.l)  GO  TO  120 

DO  30  LN=1, LINKS 

IF  (KN.NE.l)  GO  TO  10 

TDDSIM(LN)=TDD(LN) *D2R 

TDSIM(LN)=TD(LN) *D2R 

TSIM(LN)=T(LN)*D2R 

GO  TO  20 
10    CONTINUE 

TDDSIM(LN) =TDDLST(LN) 

TDSIM(LN) =TDLAST(LN) 

TSIM(LN)=TLAST(LN) 
20    CONTINUE 
30    CONTINUE 

DELTT=TIME(  KN+1 ) -TIME(  KN) 

STEPSZ=DELTT/FLOAT( ISTEP) 

DO  100  K=l, ISTEP 

TDOLD(LN) =TDSIM(LN) 

DO  40  LN=1, LINKS 

TDSIM(LN) =TDDSIM(LN) *STEPSZ+TDSIM(LN) 

TSIM(LN)=0.5*STEPSZ*(TDSIM(LN)+TDOLD(LN)) 
40    CONTINUE 
C SOLVE  FINITE  DIFFERENCE  STEPS 

CALL  KINEMT  (TSIM,PAYLD) 

DO  50  1=1,15 

B(I)=0.0 
50    CONTINUE 
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CALL  ARM1  (TSIM,TDSIM,ZERO,FMK,B) 


DO  80  1=1, LINKS 

DO  60  J=l, LINKS 

HJ(J)=0.0 

EJ(J)=0.0 

60 

CONTINUE 

EJ(I)=1.0 

CALL  ARM2  (TSIM,EJ,BJ) 

DO  70  J=l, LINKS 

H(J,I)=HJ(J) 

70 

CONTINUE 

80 
c 

CONTINUE 

DO  90  1=1, LINKS 

TDDSIM( I) =TAU( I)-B( I) 
90    CONTINUE 

IER=0 
C r COMPUTE  LINK  ACCELERATIONS 

CALL  TDDSOL  (H, 15, LINKS, LINKS, TDDSIM,15, 1,0, WORK, IER) 
100   CONTINUE 
C COMPUTE  LINK  ERROR  DIFFERENCES 

DO  110  LN=1, LINKS 

TDDLST(LN) =TDDSIM(LN) 

TDLAST(LN) =TDSIM(LN) 

TLAST(LN)=TSIM(LN) 

POSSIM(LN,KN)=TSIM(LN) *R2D 

VELSIM(LN,KN)=TDSIM(LN) *R2D 

ACLSIM(LN,KN)=TDDSIM(LN) *R2D 

ACCSIM(LN,KN)=TDDSIM(LN) *R2D 

SIMERR(LN)=SIMERR(LN)+ABS(T2(LN,KN)-ACCSIM(LN,KN)) 

T0TACC(LN)=T0TACC(LN)+ABS(T2(LN,KN)) 
110   CONTINUE 

120   CONTINUE 
RETURN 
END 
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C*************  ************************************ *»****»*******VELS0L 
(^************* ****************************  ****** ****************Y£LgQL 
(^***«* **************************************************** ******Y£LgQL 

SUBROUTINE  VELSOLU,  Y,Z,NDIM) 

REAL   X(100),Y(100),Z(100) 


SUM2=0. 

IF(NDIM-1)4,3,1 
INTEGRATION  LOOP 


1  DO  2  I=2,NDIM 
SUM1=SUM2 
SUM2=SUM2+.5*(X(I)-X(I-1))*(Y(I)+Y(I-D) 

2  Z(I-1)=SUM1 

3  Z(NDIM)=SUM2 

4  RETURN 
END 


P***** ***********  *******************************  ****** ***********pQggQL 

£* ******************************************** *******************pQggQL 

(j************ ************************************************* ***p0SS0L 
SUBROUTINE  POSSOL(X, Y,DERY,Z,NDIM) 
REAL  X(100) , Y(100) ,DERY(100) ,Z(100) 


C- 


SUM2=0. 

IF(NDIM-1)4,3,1 
INTEGRATION  LOOP 

1  DO  2  I=2,NDIM 
SUM1=SUM2 

SUM2=.5*(X(I)-X(I-1)) 
SUM2=SUM1+SUM2*((Y(I)+Y(I-1))+.3333333*SUM2*(DERY(I-1)-DERY(I))) 

2  Z(I-1)=SUM1 

3  Z(NDIM)=SUM2 

4  RETURN 
END 
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q    ********************************************************* ****«*TRANSP 

Q      ****************************************  ***********************'J1^jsJgp 

q    ******************************************************** *******XRANSP 
REAL  A(4,4,15),TT(4,4,15,2),T(15),TE(15,100,2),SMALLA(15),SMALLD(1 
15),ALPHA(15),TDUM 


INTEGER  KNOTS 


SERR1=0.0 
SERR2=0.0 
SERR3=0.0 
PI=ARSIN(1.0)*2.0 
R2D=180.0/PI 
D2R=PI/ 180.0 
LINKS=6 

DO  21    1=1, LINKS 
SMALLA(I)=0.0 
SMALLD(I)=0.0 
ALPHA(I)=0.0 
21  CONTINUE 

SMALLA(2)=43.2 

SMALLA(3)=1.9 

SMALLD(3)=12.5 

SMALLD(4)=43.2 

ALPHA(1)=-90.0*D2R 

ALPHA(3)=90.0*D2R 

ALPHA(4)=-90.0*D2R 

ALPHA(5)=90.0*D2R 


•COMPUTE   INDIVIDUAL  LINK  A-MATRICES 


c 

1=0 
7     1=1+1 

READ(  21 ,  *,  END=1 )  TDUM, TE(  1 , I, 1 ) , TE(  1 , 1, 2) 

1  CONTINUE 
READ(22,*,END=2)TDUM,TE(2,I,1),TE(2,I,2) 

2  CONTINUE 
READ(21,*,END=3)TDUM,TE(3,I,1),TE(3,I,2) 

3  CONTINUE 
READ(21,*,END=4)TDUM,TE(4,I,1),TE(4,I,2) 

4  CONTINUE 
READ(21,*,END=5)TDUM,TE(5,I,1),TE(5,I,2) 

5  CONTINUE 
READ(21,*,END=6)TDUM,TE(6,I,1),TE(6,I,2) 
GO  TO  7 

6  CONTINUE 
KNOTS=I-l 

DO  11  JJ=1, KNOTS 
DO  12  KK=1,2 
DO  10  1=1, LINKS 
T(I)=TE(I,JJ,KK)*D2R 
A(l,l,I)=COS(T(D) 
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A(1,2,I)=-C0S(ALPHA(I))*SIN(T(I)) 

A(1,3,I)=SIN(ALPHA(I))*SIN(T(I)) 

A(1,4,I)=SMALLA(I)*C0S(T(I)) 

A(2,1,I)=SIN(T(I)) 

A(2,2,I)=C0S(ALPHA(I))*C0S(T(I)) 

A(2,3, I)=-SIN(ALPHA( I) ) *COS(T( I) ) 

A(2,4,I)=SMALLA(I)*SIN(T(D) 

A(3,1,I)=0.0 

A(3,2,I)=SIN(ALPHA(I)) 

A(3,3,I)=COS(ALPHA(D) 

A(3,4,I)=SMALLD(I) 

A(4,1,I)=0.0 

A(4,2,I)=0.0 

A(4,3,I)=0.0 

A(4,4,I)=1.0 
10    CONTINUE 
C LOAD  LINK  TRANSPOSITION  T-MATRICES 

DO  111  1=1, LINKS 

DO  112  J=l,4 

DO  113  K=l,4 

TT(J,K,I,KK)=0.0 
113   CONTINUE 
112   CONTINUE 
111   CONTINUE 

DO  110  1=1, LINKS 

DO  100  J=l,4 

DO  90  K=l,4 

TT(J,K,I,KK)=0.0 

DO  80  L=l,4 

IF  (I.EQ.l)  GO  TO  60 

TT(J,K,I,KK)=TT(J,K,I,KK)+TT(J,L,I-1,KK)*A(L,K,I) 

GO  TO  70 
60    CONTINUE 

TT(J,K,1,KK)=A(J,K,1) 
70  CONTINUE 
80  CONTINUE 
90  CONTINUE 
100  CONTINUE 
110   CONTINUE 

IF  (KK.EQ.1)WRITE(61,200)TT(1,4,6,1),TT(2,4,6,1),TT(3,4,6,1) 

IF  (KK.EQ.2)WRITE(62,200)TT(1,4,6,2),TT(2,4,6,2),TT(3,4,6,2) 
12    CONTINUE 

ERR1=ABS(TT(1,4,6,1)-TT(1,4,6,2))/TT(1,4,6,1) 

ERR2=ABS(TT(2,4,6,1)-TT(2,4,6,2))/TT(2,4,6,1) 

ERR3=ABS(TT(3,4,6,1)-TT(3,4,6,2))/TT(3,4,6,1) 

WRITE ( 63 , 200) ERR1 , ERR2 , ERR3 

SERR1=ERR1+SERR1 

SERR2=ERR2+SERR2 
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SERR3=ERR3+SERR3 
11  CONTINUE 

ER1=SERR1 / FLOAT( KNOTS ) 

ER2=SERR2/FL0AT( KNOTS) 

ER3=SERR3/FLOAT( KNOTS) 

WRITE(63,230)ER1,ER2,ER3 

RETURN 
200   FORMAT  (3F15.8) 
230   FORMAT  (/'TOTAL  ERRORS  =  ',3F15.8) 

END 


C*****  ************************************************  **********iyi^j44 
q* ********************************************************** ****MAT44 
£*************************** ********************************* ***MAT44 

SUBROUTINE  MAT44  (AA, I1,BB,J1, CC,K1) 

REAL  AA(4,4,15),BB(4,4,15),CC(4,4,15) 

INTEGER  I1,J1,K1 

DO  30  J=l,4 

DO  20  K=l,4 

CC(J,K,K1)=0.0 

DO  10  L=l,4 

CC(J,K,K1)=CC(J,K,K1)+AA(J,L,I1)*BB(L,K,J1) 
10    CONTINUE 
20    CONTINUE 
30    CONTINUE 

RETURN 

END 


(3********* ***************************************************** *mat44 j 
C***** ******************************************************** **MAT44T 

£***********************************************  ****************j^<J<44T 

SUBROUTINE  MAT44T  (AA,  I1,BB,J1,CC,K1) 

REAL  AA(4,4,15),BB(4,4,15),CC(4,4,15) 

INTEGER  I1,J1,K1 

DO  30  J=l,4 

DO  20  K=l,4 

CC(J,K,K1)=0.0 

DO  10  L=l,4 

CC(J,K,K1)=CC(J,K,K1)+AA(J,L,I1)*BB(K,L,J1) 
10    CONTINUE 
20    CONTINUE 
30    CONTINUE 

RETURN 

END 
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C************************************************* ******** ******3IAT C4 

C***************************************************************^XC4 
q***************************************************************MatC4 

SUBROUTINE  MATC4  ( CI, AA, II, BB, Jl) 

REAL  AA(4,4,15),BB(4,4,15),C1 

INTEGER  II, Jl 

DO  20  1=1,4 

DO  10  J=l,4 

BB(I,J,J1)=C1*AA(I,J,I1) 
10    CONTINUE 
20    CONTINUE 

RETURN 

END 


C********* **************************** ************************  **^jqi 

C***  *******************  *********************** ******************^JQl 

q************* ******************************************** ******MATC1 

SUBROUTINE  MATC1    (AA, I1,BB,J1, CC,K1) 

REAL  AA(15),BB(4,15),CC(4,15) 

INTEGER  I1,J1,K1 

DO  10  1=1,4 

CC(I,K1)=AA(I1)*BB(I,J1) 
10    CONTINUE 

RETURN 

END 


0***** ****************************************** ****************jy^T4i 
q*****  *********************************************************  *.MAT4 1 
q****« ******************************************************* *»*MAX41 

SUBROUTINE  MAT41  (AA, I1,BB,J1, CC,K1) 

REAL  AA(4,4,15),BB(4,15),CC(4,15) 

INTEGER  I1,J1,K1 

DO  20  1=1,4 

CC(I,K1)=0.0 

DO  10  J=l,4 

CC(I,K1)=CC(I,K1)+AA(I,J,I1)*BB(J,J1) 
10    CONTINUE 
20    CONTINUE 

RETURN 

END 
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C***********************  *********************  *******************j^'j"2  4 
C********** ********************************  ********* ************^j2 4 
C************************************************************  ***MAT14 

SUBROUTINE  MAT14  ( AA, I1,BB,I1, CC,K1) 

REAL  AA(4,15),BB(4,4,15),CC(4,15) 

INTEGER  11,71, Kl 

DO  20  1=1,4 

CC(I,K1)=0.0 

DO  10  7=1,4 

CC(I,K1)=CC(I,K1)+AA(7,I1)*BB(J,I,71) 
10    CONTINUE 
20    CONTINUE 

RETURN 

END 


£***** ******* ***************  ****************** ******************jy)p44 

^*************************************  ********  ******************yy)£)44 
C********************************************* *********** *******/y)P44 

SUBROUTINE  ADD44  ( AA, I1,BB,I1, CC,K1) 

REAL  AA(4,4,15),BB(4,4,15),CC(4,4,15) 

INTEGER  11,71,11 

DO  20  1=1,4 

DO  10  7=1,4 

CC(I,7,71)=AA(I,7,I1)+BB(I,7,K1) 
10    CONTINUE 
20    CONTINUE 

RETURN 

END 


£***** *******************************  ******************** *******^^2 1 

£*  **********  **********  *************************  *****************^]T)21 

C***************************************************************^j)DU 

SUBROUTINE  ADD11  (AA,  II, BB, 71, CC,K1) 
REAL  AA(4,15),BB(4,15),CC(4,15) 
INTEGER  11,71, Kl 
DO  10  1=1,4 

CC(I,K1)=AA(I,I1)+BB(I,71) 
10    CONTINUE 
RETURN 
END 
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C********* ************** ************** ********** *************  ♦♦♦^p^xx 
C***** ************************************************** ********ADDALL 
C*** ***************************************************** *******AJ)DALL 

SUBROUTINE  ADDALL  (AA, I1,BB,J1,CC,K1,DD,L1,EE,M1) 

REAL  AA(4,4,15),BB(4,4,15),CC(4,4,15),DD(4,4,15),EE(4,4,15) 

INTEGER  I1,J1,K1,L1,M1 

DO  20  1=1,4 

DO  10  J=l,4 

EE(I,J,M1)=M(I,J,I1)+BB(I,J,J1)+CC(I,J,K1)+DD(I,J,L1) 
10    CONTINUE 
20    CONTINUE 

RETURN 

END 


£***•* ******************************** *************************  *p  j^g-p 

C* ****************************************** ********************FIRST 
(;***** ******************************************************* ***pjRS T 

SUBROUTINE  FIRST  (PTQ, II, DD, C17) 
REAL  PTQ(4,4,15),DD(4,4,15),CC1(4,4,15),C17 
INTEGER  II 

CALL  MAT44  (PTQ, II, DD, I1,CC1, II) 
C17=0.0 
DO  10  1=1,4 
C17=C17+CC1(I,I,I1) 
10    CONTINUE 
RETURN 
END 

C***** ******************************************************** **SECOND 
C** ******* ********************* ******** *************************s££OtyD 
q*************** ******  *************************************** »**SECOND 

SUBROUTINE  SECOND  (GRAV,PTQ, II, CC, C19) 

REAL  GRAV(4,15),PTQ(4,4,15),CC(4,15),CC2(4,15),C19 

INTEGER  II 

CALL  MAT14  (GRAV,I1,PTQ,I1,CC2, II) 

C19=0.0 

DO  10  J=l,4 

ci9=ci9+cc2(j,ii)*cc(j,ii) 

10    CONTINUE 
RETURN 
END 
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